Merge pull request autowarefoundation#287 from tier4/feat/improve_map…

…_based_prediction_v0.7.0

* perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum (autowarefoundation#2883)

* perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* perf(map_based_prediction): calculate yaw without calling getLaneletAngle (autowarefoundation#2895)

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: veqcc <ryuta.kambe@tier4.jp>

---------

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: Ryuta Kambe <veqcc.c@gmail.com>
Co-authored-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1173,33 +1173,53 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
       lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front());
       if (!prev_lanelets.empty()) {
         lanelet::ConstLanelet prev_lanelet = prev_lanelets.front();
+        bool init_flag = true;
+        geometry_msgs::msg::Pose prev_p;
         for (const auto & lanelet_p : prev_lanelet.centerline()) {
           geometry_msgs::msg::Pose current_p;
           current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p);
-          const double lane_yaw = lanelet::utils::getLaneletAngle(prev_lanelet, current_p.position);
+          if (init_flag) {
+            init_flag = false;
+            prev_p = current_p;
+            continue;
+          }
+
+          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 = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
           converted_path.push_back(current_p);
+          prev_p = current_p;
         }
       }
     }
 
     for (const auto & lanelet : path) {
+      bool init_flag = true;
+      geometry_msgs::msg::Pose prev_p;
       for (const auto & lanelet_p : lanelet.centerline()) {
         geometry_msgs::msg::Pose current_p;
         current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p);
-        const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, current_p.position);
-        current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
+        if (init_flag) {
+          init_flag = false;
+          prev_p = current_p;
+          continue;
+        }
 
         // Prevent from inserting same points
         if (!converted_path.empty()) {
-          const auto prev_p = converted_path.back();
-          const double tmp_dist = tier4_autoware_utils::calcDistance2d(prev_p, current_p);
+          const auto last_p = converted_path.back();
+          const double tmp_dist = tier4_autoware_utils::calcDistance2d(last_p, current_p);
           if (tmp_dist < 1e-6) {
+            prev_p = current_p;
             continue;
           }
         }
 
+        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 = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
         converted_path.push_back(current_p);
+        prev_p = current_p;
       }
     }
 

perception/map_based_prediction/src/path_generator.cpp

@@ -296,15 +296,18 @@ PosePath PathGenerator::interpolateReferencePath(
     return interpolated_path;
   }
 
-  std::vector<double> base_path_x;
-  std::vector<double> base_path_y;
-  std::vector<double> base_path_z;
-  std::vector<double> base_path_s;
+  std::vector<double> base_path_x(base_path.size());
+  std::vector<double> base_path_y(base_path.size());
+  std::vector<double> base_path_z(base_path.size());
+  std::vector<double> base_path_s(base_path.size(), 0.0);
   for (size_t i = 0; i < base_path.size(); ++i) {
-    base_path_x.push_back(base_path.at(i).position.x);
-    base_path_y.push_back(base_path.at(i).position.y);
-    base_path_z.push_back(base_path.at(i).position.z);
-    base_path_s.push_back(motion_utils::calcSignedArcLength(base_path, 0, i));
+    base_path_x.at(i) = base_path.at(i).position.x;
+    base_path_y.at(i) = base_path.at(i).position.y;
+    base_path_z.at(i) = base_path.at(i).position.z;
+    if (i > 0) {
+      base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d(
+                                                    base_path.at(i - 1), base_path.at(i));
+    }
   }
 
   std::vector<double> resampled_s(frenet_predicted_path.size());