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

fix: improve pedestrian path generation efficiency

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

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(); ++i) {
+  for (size_t i = 0; i < predicted_path.path.size() - 1; ++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,14 +48,18 @@ 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 = object.kinematics.pose_with_covariance.pose.orientation;
+    world_frame_pose.orientation = pedestrian_to_entry_point_orientation;
     predicted_path.path.push_back(world_frame_pose);
     if (predicted_path.path.size() >= predicted_path.path.max_size()) {
       break;
@@ -87,43 +91,39 @@ 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 = object.kinematics.pose_with_covariance.pose.orientation;
+      world_frame_pose.orientation = pedestrian_to_entry_point_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 = object.kinematics.pose_with_covariance.pose.orientation;
+      world_frame_pose.orientation = entry_to_exit_point_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_);