diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 01b6b3d278a33..dd87bbc14705d 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/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] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 4ac5fd76babea..dc6c74c25a539 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/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 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 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 dd18083af88b9..8dd4a0956b9f5 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/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( "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound"); + + num_continuous_state_transition_ = + declare_parameter("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(object_info.size()) - 2).output_maneuver; + + for (int i = 0; + i < std::min(num_continuous_state_transition_, static_cast(object_info.size())); ++i) { + const auto & tmp_maneuver = + object_info.at(static_cast(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(