ordered_multi_queue.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #ifndef CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_
18 #define CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_
19 
20 #include <functional>
21 #include <map>
22 #include <memory>
23 #include <string>
24 #include <tuple>
25 
30 
31 namespace cartographer {
32 namespace sensor {
33 
34 struct QueueKey {
36  std::string sensor_id;
37 
38  bool operator<(const QueueKey& other) const {
39  return std::forward_as_tuple(trajectory_id, sensor_id) <
40  std::forward_as_tuple(other.trajectory_id, other.sensor_id);
41  }
42 };
43 
44 // Maintains multiple queues of sorted sensor data and dispatches it in merge
45 // sorted order. It will wait to see at least one value for each unfinished
46 // queue before dispatching the next time ordered value across all queues.
47 //
48 // This class is thread-compatible.
50  public:
51  using Callback = std::function<void(std::unique_ptr<Data>)>;
52 
55 
56  // Adds a new queue with key 'queue_key' which must not already exist.
57  // 'callback' will be called whenever data from this queue can be dispatched.
58  void AddQueue(const QueueKey& queue_key, Callback callback);
59 
60  // Marks a queue as finished, i.e. no further data can be added. The queue
61  // will be removed once the last piece of data from it has been dispatched.
62  void MarkQueueAsFinished(const QueueKey& queue_key);
63 
64  // Adds 'data' to a queue with the given 'queue_key'. Data must be added
65  // sorted per queue.
66  void Add(const QueueKey& queue_key, std::unique_ptr<Data> data);
67 
68  // Dispatches all remaining values in sorted order and removes the underlying
69  // queues.
70  void Flush();
71 
72  // Must only be called if at least one unfinished queue exists. Returns the
73  // key of a queue that needs more data before the OrderedMultiQueue can
74  // dispatch data.
75  QueueKey GetBlocker() const;
76 
77  private:
78  struct Queue {
81  bool finished = false;
82  };
83 
84  void Dispatch();
85  void CannotMakeProgress(const QueueKey& queue_key);
86  common::Time GetCommonStartTime(int trajectory_id);
87 
88  // Used to verify that values are dispatched in sorted order.
89  common::Time last_dispatched_time_ = common::Time::min();
90 
91  std::map<int, common::Time> common_start_time_per_trajectory_;
92  std::map<QueueKey, Queue> queues_;
94 };
95 
96 } // namespace sensor
97 } // namespace cartographer
98 
99 #endif // CARTOGRAPHER_SENSOR_INTERNAL_ORDERED_MULTI_QUEUE_H_
std::function< void(std::unique_ptr< Data >)> Callback
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
common::BlockingQueue< std::unique_ptr< Data > > queue
bool operator<(const QueueKey &other) const
std::map< int, common::Time > common_start_time_per_trajectory_


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58