feat(map_based_prediction): disable time delay compensation (autoware…

…foundation#3806)

* disable time delay compensation

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix unused parameter

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix lane change decision not working problem due to empty future_lanenet in unconnected lanelets

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1292,7 +1292,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
 
 Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
-  const double object_detected_time)
+  const double /*object_detected_time*/)
 {
   // Step1. Check if we have the object in the buffer
   const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
@@ -1322,7 +1322,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
 
   // Step3. Get closest previous lanelet ID
   const auto & prev_info = object_info.at(static_cast<size_t>(prev_id));
-  const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay);
+  const auto prev_pose = prev_info.pose;
   const lanelet::ConstLanelets prev_lanelets =
     object_info.at(static_cast<size_t>(prev_id)).current_lanelets;
   if (prev_lanelets.empty()) {
@@ -1342,19 +1342,20 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
 
   // Step4. Check if the vehicle has changed lane
   const auto current_lanelet = current_lanelet_data.lanelet;
-  const double current_time_delay = std::max(current_time - object_detected_time, 0.0);
-  const auto current_pose = compensateTimeDelay(
-    object.kinematics.pose_with_covariance.pose, object.kinematics.twist_with_covariance.twist,
-    current_time_delay);
+  const auto current_pose = object.kinematics.pose_with_covariance.pose;
   const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose);
   lanelet::routing::LaneletPaths possible_paths =
     routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false);
   bool has_lane_changed = true;
-  for (const auto & path : possible_paths) {
-    for (const auto & lanelet : path) {
-      if (lanelet == current_lanelet) {
-        has_lane_changed = false;
-        break;
+  if (prev_lanelet == current_lanelet) {
+    has_lane_changed = false;
+  } else {
+    for (const auto & path : possible_paths) {
+      for (const auto & lanelet : path) {
+        if (lanelet == current_lanelet) {
+          has_lane_changed = false;
+          break;
+        }
       }
     }
   }