| 218 | } |
| 219 | |
| 220 | std::optional<NodeId> LoopClosureModule::getQueryAgentId(size_t stamp_ns) { |
| 221 | if (agent_queue_.empty()) { |
| 222 | return std::nullopt; |
| 223 | } |
| 224 | |
| 225 | const auto& node = lcd_graph_->getDynamicNode(agent_queue_.top())->get(); |
| 226 | const auto prev_time = node.timestamp; |
| 227 | |
| 228 | if (!node.hasParent()) { |
| 229 | LOG(ERROR) << "Found agent node without parent: " |
| 230 | << NodeSymbol(agent_queue_.top()).getLabel() << ". Discarding!"; |
| 231 | agent_queue_.pop(); |
| 232 | return std::nullopt; |
| 233 | } |
| 234 | |
| 235 | std::chrono::duration<double> diff_s = std::chrono::nanoseconds(stamp_ns) - prev_time; |
| 236 | // we consider should_shutdown_ here to make sure we're not waiting on popping from |
| 237 | // the LCD queue while not getting new place messages |
| 238 | if (!should_shutdown_ && diff_s.count() < config_.lcd_agent_horizon_s) { |
| 239 | return std::nullopt; |
| 240 | } |
| 241 | |
| 242 | if (should_shutdown_ && (diff_s.count() < config_.lcd_agent_horizon_s)) { |
| 243 | LOG(ERROR) << "Forcing pop of node " << NodeSymbol(agent_queue_.top()).getLabel() |
| 244 | << " from lcd queue due to shutdown: " |
| 245 | << ", diff: " << diff_s.count() << " / " << config_.lcd_agent_horizon_s; |
| 246 | } |
| 247 | |
| 248 | auto valid_node = agent_queue_.top(); |
| 249 | agent_queue_.pop(); |
| 250 | return valid_node; |
| 251 | } |
| 252 | |
| 253 | } // namespace hydra |