feat(map_based_prediction): suppress lane change decision flickering (a…

…utowarefoundation#3788)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -23,5 +23,6 @@
         dist_ratio_threshold_to_right_bound: 0.5 #[ratio
         diff_dist_threshold_to_left_bound: 0.29 #[m]
         diff_dist_threshold_to_right_bound: -0.29 #[m]
+      num_continuous_state_transition: 3
 
     reference_path_resolution: 0.5 #[m]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -59,6 +59,13 @@ struct LateralKinematicsToLanelet
   double filtered_right_lateral_velocity;
 };
 
+enum class Maneuver {
+  UNINITIALIZED = 0,
+  LANE_FOLLOW = 1,
+  LEFT_LANE_CHANGE = 2,
+  RIGHT_LANE_CHANGE = 3,
+};
+
 struct ObjectData
 {
   std_msgs::msg::Header header;
@@ -69,12 +76,9 @@ struct ObjectData
   double time_delay;
   // for lane change prediction
   std::unordered_map<lanelet::ConstLanelet, LateralKinematicsToLanelet> lateral_kinematics_set;
-};
-
-enum class Maneuver {
-  LANE_FOLLOW = 0,
-  LEFT_LANE_CHANGE = 1,
-  RIGHT_LANE_CHANGE = 2,
+  Maneuver one_shot_maneuver{Maneuver::UNINITIALIZED};
+  Maneuver output_maneuver{
+    Maneuver::UNINITIALIZED};  // output maneuver considering previous one shot maneuvers
 };
 
 struct LaneletData
@@ -153,6 +157,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   double dist_ratio_threshold_to_right_bound_;
   double diff_dist_threshold_to_left_bound_;
   double diff_dist_threshold_to_right_bound_;
+  int num_continuous_state_transition_;
   double reference_path_resolution_;
 
   // Stop watch

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -517,6 +517,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound");
     diff_dist_threshold_to_right_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound");
+
+    num_continuous_state_transition_ =
+      declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
   }
   reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5);
 
@@ -1170,14 +1173,50 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
   const double object_detected_time)
 {
-  if (lane_change_detection_method_ == "time_to_change_lane") {
-    return predictObjectManeuverByTimeToLaneChange(
-      object, current_lanelet_data, object_detected_time);
-  } else if (lane_change_detection_method_ == "lat_diff_distance") {
-    return predictObjectManeuverByLatDiffDistance(
-      object, current_lanelet_data, object_detected_time);
-  }
-  throw std::logic_error("Lane change detection method is invalid.");
+  // calculate maneuver
+  const auto current_maneuver = [&]() {
+    if (lane_change_detection_method_ == "time_to_change_lane") {
+      return predictObjectManeuverByTimeToLaneChange(
+        object, current_lanelet_data, object_detected_time);
+    } else if (lane_change_detection_method_ == "lat_diff_distance") {
+      return predictObjectManeuverByLatDiffDistance(
+        object, current_lanelet_data, object_detected_time);
+    }
+    throw std::logic_error("Lane change detection method is invalid.");
+  }();
+
+  const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
+  if (objects_history_.count(object_id) == 0) {
+    return current_maneuver;
+  }
+  auto & object_info = objects_history_.at(object_id);
+
+  // update maneuver in object history
+  if (!object_info.empty()) {
+    object_info.back().one_shot_maneuver = current_maneuver;
+  }
+
+  // decide maneuver considering previous results
+  if (object_info.size() < 2) {
+    object_info.back().output_maneuver = current_maneuver;
+    return current_maneuver;
+  }
+  // NOTE: The index of previous maneuver is not object_info.size() - 1
+  const auto prev_output_maneuver =
+    object_info.at(static_cast<int>(object_info.size()) - 2).output_maneuver;
+
+  for (int i = 0;
+       i < std::min(num_continuous_state_transition_, static_cast<int>(object_info.size())); ++i) {
+    const auto & tmp_maneuver =
+      object_info.at(static_cast<int>(object_info.size()) - 1 - i).one_shot_maneuver;
+    if (tmp_maneuver != current_maneuver) {
+      object_info.back().output_maneuver = prev_output_maneuver;
+      return prev_output_maneuver;
+    }
+  }
+
+  object_info.back().output_maneuver = current_maneuver;
+  return current_maneuver;
 }
 
 Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(