From 9080c1655157524d06c7c929d91987c2071092c6 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Wed, 24 May 2023 20:29:48 +0900 Subject: [PATCH] feat(map_based_prediction): disable time delay compensation (#3806) * disable time delay compensation Signed-off-by: yoshiri * fix unused parameter Signed-off-by: yoshiri * fix lane change decision not working problem due to empty future_lanenet in unconnected lanelets Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../src/map_based_prediction_node.cpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 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 index 8dd4a0956b9f5..68f31593116f7 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/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(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(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; + } } } }