From f23916f6e2881be89dc800e5da5f701c61a2066e Mon Sep 17 00:00:00 2001
From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com>
Date: Wed, 27 Nov 2024 11:26:56 +0900
Subject: [PATCH] fix(autoware_trajectory): autoware_trajectory bug (#9475)

fix autoware_trajectory bug

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 common/autoware_trajectory/src/path_point.cpp              | 2 +-
 common/autoware_trajectory/src/path_point_with_lane_id.cpp | 2 +-
 common/autoware_trajectory/src/pose.cpp                    | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp
index af1e9286b533d..4a2ac80b749bd 100644
--- a/common/autoware_trajectory/src/path_point.cpp
+++ b/common/autoware_trajectory/src/path_point.cpp
@@ -76,7 +76,7 @@ std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points)
   points.reserve(bases.size());
   for (const auto & s : bases) {
     PointType p;
-    p.pose = Trajectory<geometry_msgs::msg::Pose>::compute(s);
+    p.pose = Trajectory<geometry_msgs::msg::Pose>::compute(s - start_);
     p.longitudinal_velocity_mps = static_cast<float>(this->longitudinal_velocity_mps.compute(s));
     p.lateral_velocity_mps = static_cast<float>(this->lateral_velocity_mps.compute(s));
     p.heading_rate_rps = static_cast<float>(this->heading_rate_rps.compute(s));
diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp
index 6c983c4dfa0d3..c3d7441fef405 100644
--- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp
+++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp
@@ -65,7 +65,7 @@ std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points)
   points.reserve(bases.size());
   for (const auto & s : bases) {
     PointType p;
-    p.point = BaseClass::compute(s);
+    p.point = BaseClass::compute(s - start_);
     p.lane_ids = lane_ids.compute(s);
     points.emplace_back(p);
   }
diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp
index 35eca81981f89..3906ee17fa2eb 100644
--- a/common/autoware_trajectory/src/pose.cpp
+++ b/common/autoware_trajectory/src/pose.cpp
@@ -92,7 +92,7 @@ std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points)
   points.reserve(bases.size());
   for (const auto & s : bases) {
     PointType p;
-    p.position = BaseClass::compute(s);
+    p.position = BaseClass::compute(s - start_);
     p.orientation = orientation_interpolator_->compute(s);
     points.emplace_back(p);
   }