perf(map_based_prediction): calculate yaw without calling getLaneletA…

…ngle (autowarefoundation#2895) Signed-off-by: veqcc <ryuta.kambe@tier4.jp> Co-authored-by: veqcc <ryuta.kambe@tier4.jp>
1222-takeshi · Feb 27, 2023 · 21cfb7a · 21cfb7a
1 parent b93918e
commit 21cfb7a
Showing 1 changed file with 25 additions and 5 deletions.
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/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;
       }
     }