Revert "fix(map_based_prediction): improve pedestrian path generation efficiency (#6964)"

This reverts commit c7c826d.

Author: satoshi-ota

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1145,7 +1145,7 @@ bool MapBasedPredictionNode::doesPathCrossFence(
   const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line)
 {
   // check whether the predicted path cross with fence
-  for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) {
+  for (size_t i = 0; i < predicted_path.path.size(); ++i) {
     for (size_t j = 0; j < fence_line.size() - 1; ++j) {
       if (isIntersecting(
             predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j],

perception/map_based_prediction/src/path_generator.cpp

@@ -48,18 +48,14 @@ PredictedPath PathGenerator::generatePathToTargetPoint(
   const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
   const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;
 
-  const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized();
-  const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
-    std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x()));
-
   for (double dt = 0.0; dt < arrival_time + ep; dt += sampling_time_interval_) {
     geometry_msgs::msg::Pose world_frame_pose;
     world_frame_pose.position.x =
-      obj_pos.x + velocity * pedestrian_to_entry_point_normalized.x() * dt;
+      obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt;
     world_frame_pose.position.y =
-      obj_pos.y + velocity * pedestrian_to_entry_point_normalized.y() * dt;
+      obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt;
     world_frame_pose.position.z = obj_pos.z;
-    world_frame_pose.orientation = pedestrian_to_entry_point_orientation;
+    world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
     predicted_path.path.push_back(world_frame_pose);
     if (predicted_path.path.size() >= predicted_path.path.max_size()) {
       break;
@@ -91,39 +87,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
   const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
   const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;
 
-  const auto pedestrian_to_entry_point_normalized = pedestrian_to_entry_point.normalized();
-  const auto pedestrian_to_entry_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
-    std::atan2(pedestrian_to_entry_point_normalized.y(), pedestrian_to_entry_point_normalized.x()));
-  const auto entry_to_exit_point_normalized = entry_to_exit_point.normalized();
-  const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw(
-    std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x()));
-
   for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) {
     geometry_msgs::msg::Pose world_frame_pose;
     if (dt < arrival_time) {
       world_frame_pose.position.x =
-        obj_pos.x + velocity * pedestrian_to_entry_point_normalized.x() * dt;
+        obj_pos.x + velocity * pedestrian_to_entry_point.normalized().x() * dt;
       world_frame_pose.position.y =
-        obj_pos.y + velocity * pedestrian_to_entry_point_normalized.y() * dt;
+        obj_pos.y + velocity * pedestrian_to_entry_point.normalized().y() * dt;
       world_frame_pose.position.z = obj_pos.z;
-      world_frame_pose.orientation = pedestrian_to_entry_point_orientation;
+      world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
       predicted_path.path.push_back(world_frame_pose);
     } else {
       world_frame_pose.position.x =
         reachable_crosswalk.front_center_point.x() +
-        velocity * entry_to_exit_point_normalized.x() * (dt - arrival_time);
+        velocity * entry_to_exit_point.normalized().x() * (dt - arrival_time);
       world_frame_pose.position.y =
         reachable_crosswalk.front_center_point.y() +
-        velocity * entry_to_exit_point_normalized.y() * (dt - arrival_time);
+        velocity * entry_to_exit_point.normalized().y() * (dt - arrival_time);
       world_frame_pose.position.z = obj_pos.z;
-      world_frame_pose.orientation = entry_to_exit_point_orientation;
+      world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
       predicted_path.path.push_back(world_frame_pose);
     }
     if (predicted_path.path.size() >= predicted_path.path.max_size()) {
       break;
     }
   }
 
+  // calculate orientation of each point
+  if (predicted_path.path.size() >= 2) {
+    for (size_t i = 0; i < predicted_path.path.size() - 1; i++) {
+      const auto yaw = tier4_autoware_utils::calcAzimuthAngle(
+        predicted_path.path.at(i).position, predicted_path.path.at(i + 1).position);
+      predicted_path.path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
+    }
+    predicted_path.path.back().orientation =
+      predicted_path.path.at(predicted_path.path.size() - 2).orientation;
+  }
+
   predicted_path.confidence = 1.0;
   predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_);