From 9eff11e04cfd89295d47e35ee6b37f1c474c69c7 Mon Sep 17 00:00:00 2001
From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com>
Date: Wed, 19 Feb 2025 21:14:35 +0900
Subject: [PATCH] feat(autoware_trajecotry): add a conversion function from
 point trajectory to pose trajectory (#207)

feat(autoware_trajecotry): add conversion function from point trajectory to pose trajectory

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../include/autoware/trajectory/point.hpp     |  3 +++
 .../include/autoware/trajectory/pose.hpp      |  3 +++
 common/autoware_trajectory/src/pose.cpp       | 27 +++++++++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp
index af47c404f7..cc69ffc2ae 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp
@@ -36,6 +36,9 @@ namespace autoware::trajectory
 template <>
 class Trajectory<geometry_msgs::msg::Point>
 {
+  template <class PointType>
+  friend class Trajectory;
+
   using PointType = geometry_msgs::msg::Point;
 
 protected:
diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
index 911db11bf8..a7030e352b 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
@@ -49,6 +49,9 @@ class Trajectory<geometry_msgs::msg::Pose> : public Trajectory<geometry_msgs::ms
   Trajectory & operator=(const Trajectory & rhs);
   Trajectory & operator=(Trajectory && rhs) = default;
 
+  // enable making trajectory from point trajectory
+  explicit Trajectory(const Trajectory<geometry_msgs::msg::Point> & point_trajectory);
+
   bool build(const std::vector<PointType> & points);
 
   /**
diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp
index 0dc3287769..37dca2f24a 100644
--- a/common/autoware_trajectory/src/pose.cpp
+++ b/common/autoware_trajectory/src/pose.cpp
@@ -15,6 +15,7 @@
 #include "autoware/trajectory/pose.hpp"
 
 #include "autoware/trajectory/detail/helpers.hpp"
+#include "autoware/trajectory/forward.hpp"
 #include "autoware/trajectory/interpolator/spherical_linear.hpp"
 
 #include <tf2/LinearMath/Quaternion.h>
@@ -45,6 +46,32 @@ Trajectory<PointType> & Trajectory<PointType>::operator=(const Trajectory & rhs)
   return *this;
 }
 
+Trajectory<PointType>::Trajectory(const Trajectory<geometry_msgs::msg::Point> & point_trajectory)
+: Trajectory()
+{
+  x_interpolator_ = point_trajectory.x_interpolator_->clone();
+  y_interpolator_ = point_trajectory.y_interpolator_->clone();
+  z_interpolator_ = point_trajectory.z_interpolator_->clone();
+  bases_ = point_trajectory.get_internal_bases();
+  start_ = point_trajectory.start_;
+  end_ = point_trajectory.end_;
+
+  // build mock orientations
+  std::vector<geometry_msgs::msg::Quaternion> orientations(bases_.size());
+  for (size_t i = 0; i < bases_.size(); ++i) {
+    orientations[i].w = 1.0;
+  }
+  bool success = orientation_interpolator_->build(bases_, orientations);
+
+  if (!success) {
+    throw std::runtime_error(
+      "Failed to build orientation interpolator.");  // This Exception should not be thrown.
+  }
+
+  // align orientation with trajectory direction
+  align_orientation_with_trajectory_direction();
+}
+
 bool Trajectory<PointType>::build(const std::vector<PointType> & points)
 {
   std::vector<geometry_msgs::msg::Point> path_points;