fix(behavior_path_planner): lane change interpolated obj orientation

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -155,8 +155,21 @@ double getArcLengthToTargetLanelet(
   const Pose & pose);
 
 // object collision check
-
-Pose lerpByPose(const Pose & p1, const Pose & p2, const double t);
+inline Pose lerpByPose(const Pose & p1, const Pose & p2, const double t)
+{
+  tf2::Transform tf_transform1;
+  tf2::Transform tf_transform2;
+  tf2::fromMsg(p1, tf_transform1);
+  tf2::fromMsg(p2, tf_transform2);
+  const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t);
+  const auto & tf_quaternion =
+    tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t);
+
+  Pose pose{};
+  pose.position = tf2::toMsg(tf_point, pose.position);
+  pose.orientation = tf2::toMsg(tf_quaternion);
+  return pose;
+}
 
 inline Point lerpByPoint(const Point & p1, const Point & p2, const double t)
 {
@@ -195,6 +208,28 @@ Point lerpByLength(const std::vector<T> & point_array, const double length)
   return tier4_autoware_utils::getPoint(point_array.back());
 }
 
+template <class T>
+Pose lerpPoseByLength(const std::vector<T> & point_array, const double length)
+{
+  Pose lerped_pose;
+  if (point_array.empty()) {
+    return lerped_pose;
+  }
+  Pose prev_geom_pose = tier4_autoware_utils::getPose(point_array.front());
+  double accumulated_length = 0;
+  for (const auto & pt : point_array) {
+    const auto & geom_pose = tier4_autoware_utils::getPose(pt);
+    const double distance = tier4_autoware_utils::calcDistance3d(prev_geom_pose, geom_pose);
+    if (accumulated_length + distance > length) {
+      return lerpByPose(prev_geom_pose, geom_pose, (length - accumulated_length) / distance);
+    }
+    accumulated_length += distance;
+    prev_geom_pose = geom_pose;
+  }
+
+  return tier4_autoware_utils::getPose(point_array.back());
+}
+
 bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt);
 
 bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon);

planning/behavior_path_planner/src/utilities.cpp

@@ -197,10 +197,7 @@ PredictedPath convertToPredictedPath(
   double prev_vehicle_speed = vehicle_speed;
 
   // first point
-  const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length);
-  Pose predicted_pose;
-  predicted_pose.position = pt;
-  predicted_path.path.push_back(predicted_pose);
+  predicted_path.path.push_back(lerpPoseByLength(path.points, vehicle_pose_frenet.length));
 
   for (double t = resolution; t < duration; t += resolution) {
     double accelerated_velocity = prev_vehicle_speed + acceleration * t;
@@ -213,10 +210,8 @@ PredictedPath convertToPredictedPath(
     }
 
     length += travel_distance;
-    const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length + length);
-    Pose predicted_pose;
-    predicted_pose.position = pt;
-    predicted_path.path.push_back(predicted_pose);
+    predicted_path.path.push_back(
+      lerpPoseByLength(path.points, vehicle_pose_frenet.length + length));
     prev_vehicle_speed = accelerated_velocity;
   }
   return predicted_path;
@@ -238,21 +233,6 @@ PredictedPath resamplePredictedPath(
   return resampled_path;
 }
 
-Pose lerpByPose(const Pose & p1, const Pose & p2, const double t)
-{
-  tf2::Transform tf_transform1, tf_transform2;
-  tf2::fromMsg(p1, tf_transform1);
-  tf2::fromMsg(p2, tf_transform2);
-  const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t);
-  const auto & tf_quaternion =
-    tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t);
-
-  Pose pose{};
-  pose.position = tf2::toMsg(tf_point, pose.position);
-  pose.orientation = tf2::toMsg(tf_quaternion);
-  return pose;
-}
-
 bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * lerped_pt)
 {
   const rclcpp::Duration time_step(path.time_step);
@@ -2311,13 +2291,12 @@ void getProjectedDistancePointFromPolygons(
 }
 
 bool getEgoExpectedPoseAndConvertToPolygon(
-  const Pose & current_pose, const PredictedPath & pred_path,
+  [[maybe_unused]] const Pose & current_pose, const PredictedPath & pred_path,
   tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time,
   const VehicleInfo & ego_info, Pose & expected_pose, std::string & failed_reason)
 {
   bool is_lerped =
     util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason);
-  expected_pose.orientation = current_pose.orientation;
 
   const auto & i = ego_info;
   const auto & front_m = i.max_longitudinal_offset_m;
@@ -2336,7 +2315,6 @@ bool getObjectExpectedPoseAndConvertToPolygon(
 {
   bool is_lerped =
     util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose, failed_reason);
-  expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
 
   is_lerped = util::calcObjectPolygon(object.shape, expected_pose, &obj_polygon);
   return is_lerped;