From 5bffe899a1ad0239703ef23f99cf02f05501dda5 Mon Sep 17 00:00:00 2001
From: Daniel <danielsanchezaran@gmail.com>
Date: Wed, 5 Mar 2025 09:34:03 +0900
Subject: [PATCH] add method to_point for TrajectoryPoint type

Signed-off-by: Daniel <danielsanchezaran@gmail.com>
---
 .../include/autoware/trajectory/detail/types.hpp     |  4 ++++
 common/autoware_trajectory/src/detail/types.cpp      | 12 ++++++++++++
 2 files changed, 16 insertions(+)

diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp
index f4150b7e37..dd64a7d3eb 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp
@@ -20,7 +20,9 @@
 #include <Eigen/Core>
 
 #include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -63,12 +65,14 @@ struct ImmutablePoint3d : ImmutablePoint2d
 MutablePoint3d to_point(geometry_msgs::msg::Point & p);
 MutablePoint3d to_point(geometry_msgs::msg::Pose & p);
 MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p);
+MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p);
 MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p);
 MutablePoint2d to_point(lanelet::BasicPoint2d & p);
 
 ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p);
 ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p);
 ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p);
+ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p);
 ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p);
 ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p);
 
diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp
index 9c3174f608..dec4205f25 100644
--- a/common/autoware_trajectory/src/detail/types.cpp
+++ b/common/autoware_trajectory/src/detail/types.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/trajectory/detail/types.hpp"
 
+#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
+
 namespace autoware::trajectory::detail
 {
 MutablePoint3d to_point(geometry_msgs::msg::Point & p)
@@ -31,6 +33,11 @@ MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p)
   return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
 }
 
+MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p)
+{
+  return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
+}
+
 MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z};
@@ -56,6 +63,11 @@ ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p)
   return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
 }
 
+ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p)
+{
+  return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
+}
+
 ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z};