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 30f9976655e5d..a6c037c62f722 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 @@ -367,37 +367,6 @@ class MapBasedPredictionNode : public rclcpp::Node } return true; }; - struct ObjectAccelerationMonitor - { - std::vector tracked_objects_; - double smoothing_factor_ = 0.5; - - double getFilteredAcceleration(const TrackedObject & object) - { - const double current_acceleration = std::hypot( - object.kinematics.acceleration_with_covariance.accel.linear.x, - object.kinematics.acceleration_with_covariance.accel.linear.y); - - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); - const auto prev_object_info = - std::find_if(tracked_objects_.begin(), tracked_objects_.end(), [&](const auto & object) { - return tier4_autoware_utils::toHexString(object.object_id) == uuid; - }); - - if (prev_object_info == tracked_objects_.end()) { - tracked_objects_.push_back(object); - return current_acceleration; - } - const double prev_acceleration = std::hypot( - prev_object_info->kinematics.acceleration_with_covariance.accel.linear.x, - prev_object_info->kinematics.acceleration_with_covariance.accel.linear.y); - const double filtered_acceleration = - smoothing_factor_ * current_acceleration + (1.0 - smoothing_factor_) * prev_acceleration; - *prev_object_info = object; - return filtered_acceleration; - } - }; - ObjectAccelerationMonitor object_acceleration_monitor_; }; } // namespace map_based_prediction