fix(autoware_trajectory): autoware_trajectory bug (#9475)

fix autoware_trajectory bug Signed-off-by: Y.Hisaki <[email protected]>
autowarefoundation · Nov 27, 2024 · f23916f · f23916f
1 parent 5b5a93c
commit f23916f
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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);
   }