revert (map_based_prediction): use linear interpolation for path conv…

…ersion (#8400)" (#8417) Revert "perf(map_based_prediction): use linear interpolation for path conversion (#8400)" This reverts commit 147403f. Signed-off-by: Taekjin LEE <[email protected]>
autowarefoundation · Aug 9, 2024 · ea52340 · ea52340
1 parent e9dcf99
commit ea52340
Showing 1 changed file with 8 additions and 26 deletions.
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -2402,15 +2402,9 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
             continue;
           }
 
-          const double dx = current_p.position.x - prev_p.position.x;
-          const double dy = current_p.position.y - prev_p.position.y;
-          const double sin_yaw_half = dy / (std::sqrt(dx * dx + dy * dy) + dx);
-          const double cos_yaw_half = std::sqrt(1 - sin_yaw_half * sin_yaw_half);
-          current_p.orientation.x = 0.0;
-          current_p.orientation.y = 0.0;
-          current_p.orientation.z = sin_yaw_half;
-          current_p.orientation.w = cos_yaw_half;
-
+          const double lane_yaw = std::atan2(
+            current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
+          current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw);
           converted_path.push_back(current_p);
           prev_p = current_p;
         }
@@ -2439,29 +2433,17 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
           }
         }
 
-        const double dx = current_p.position.x - prev_p.position.x;
-        const double dy = current_p.position.y - prev_p.position.y;
-        const double sin_yaw_half = dy / (std::sqrt(dx * dx + dy * dy) + dx);
-        const double cos_yaw_half = std::sqrt(1 - sin_yaw_half * sin_yaw_half);
-        current_p.orientation.x = 0.0;
-        current_p.orientation.y = 0.0;
-        current_p.orientation.z = sin_yaw_half;
-        current_p.orientation.w = cos_yaw_half;
-
+        const double lane_yaw = std::atan2(
+          current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
+        current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw);
         converted_path.push_back(current_p);
         prev_p = current_p;
       }
     }
 
     // Resample Path
-    const bool use_akima_spline_for_xy = true;
-    const bool use_lerp_for_z = true;
-    // the options use_akima_slpine_for_xy and use_lerp_for_z are set to true
-    // but the implementation of use_akima_slpine_for_xy in resamplePoseVector and
-    // resamplePointVector is opposite to the options so the options are set to true to use linear
-    // interpolation for xy
-    const auto resampled_converted_path = autoware::motion_utils::resamplePoseVector(
-      converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z);
+    const auto resampled_converted_path =
+      autoware::motion_utils::resamplePoseVector(converted_path, reference_path_resolution_);
     converted_paths.push_back(resampled_converted_path);
   }