99 {
100 std::lock_guard<std::mutex> lock(buffer_mutex_);
101 if (!init_) {
102 AERROR <<
"msg buffer is uninitialized.";
103 return false;
104 }
105 if (buffer_queue_.empty()) {
106 AERROR <<
"msg buffer is empty.";
107 return false;
108 }
109 if (buffer_queue_.front().first - FLAGS_obs_buffer_match_precision >
110 timestamp) {
112 << ") is earlier than the oldest timestamp ("
113 << buffer_queue_.front().first << ").";
114 return false;
115 }
116 if (buffer_queue_.back().first + FLAGS_obs_buffer_match_precision <
117 timestamp) {
119 << ") is newer than the latest timestamp ("
120 << buffer_queue_.back().first << ").";
121 return false;
122 }
123
124
125 double distance = std::numeric_limits<double>::max();
126 int idx = static_cast<int>(buffer_queue_.size()) - 1;
127 for (; idx >= 0; --idx) {
128 double temp_distance = fabs(timestamp - buffer_queue_[idx].first);
129 if (temp_distance >= distance) {
130 break;
131 }
132 distance = temp_distance;
133 }
134 *msg = buffer_queue_[idx + 1].second;
135
136 return true;
137}