test_priority_mux_base.cpp
Go to the documentation of this file.
1 // SPDX-License-Identifier: BSD-3-Clause
2 // SPDX-FileCopyrightText: Czech Technical University in Prague
3 
10 #include "gtest/gtest.h"
11 
12 #include <memory>
13 #include <string>
14 #include <unordered_map>
15 
16 #include <cras_cpp_common/log_utils/memory.h>
17 #include <ros/duration.h>
18 #include <ros/time.h>
19 
20 #include <cras_topic_tools/priority_mux_base.h>
21 
22 using namespace cras;
23 
24 TEST(PriorityMuxBase, OneOutput) // NOLINT
25 {
26  auto log = std::make_shared<cras::MemoryLogHelper>();
27 
28  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
29  {"o1p10", {"o1p10", "o1p10", "o1", 10, {1, 0}}},
30  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
31  {"o1p30", {"o1p30", "o1p30", "o1", 30, {1, 0}}},
32  };
33 
34  std::string timerName;
35  ros::Duration timerTimeout;
36  bool timerCalled {false};
37  auto setTimer = [&](const std::string& name, const ros::Duration& timeout)
38  {
39  timerName = name;
40  timerTimeout = timeout;
41  timerCalled = true;
42  };
43 
44  PriorityMux mux(topicConfigs, {}, setTimer, ros::Time(0), log);
45 
46  EXPECT_EQ(0, mux.getActivePriority());
47  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
48  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
49  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
50 
51  // prio 10, none is active => prio 10 selected
52  EXPECT_TRUE(mux.cb("o1p10", ros::Time(1), ros::Time(1)));
53 
54  EXPECT_EQ(10, mux.getActivePriority());
55  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
56  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
57  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
58  EXPECT_EQ("o1p10", timerName);
59  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
60 
61  timerName = {};
62  timerTimeout = {};
63  timerCalled = false;
64 
65  // prio 10 before timeout, should still be selected
66  mux.update(ros::Time(1.5));
67 
68  EXPECT_EQ(10, mux.getActivePriority());
69  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
70  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
71  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
72  EXPECT_FALSE(timerCalled);
73 
74  // prio 10 timed out => none selected
75  mux.update(ros::Time(2.0));
76 
77  EXPECT_EQ(0, mux.getActivePriority());
78  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
79  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
80  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
81  EXPECT_FALSE(timerCalled);
82 
83  // prio 10, none is active => prio 10 selected
84  EXPECT_TRUE(mux.cb("o1p10", ros::Time(3), ros::Time(3)));
85 
86  EXPECT_EQ(10, mux.getActivePriority());
87  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
88  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
89  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
90  EXPECT_EQ("o1p10", timerName);
91  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
92 
93  timerName = {};
94  timerTimeout = {};
95  timerCalled = false;
96 
97  // prio 10, 10 active => prio 10 selected
98  EXPECT_TRUE(mux.cb("o1p10", ros::Time(3.1), ros::Time(3.1)));
99 
100  EXPECT_EQ(10, mux.getActivePriority());
101  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
102  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
103  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
104  EXPECT_EQ("o1p10", timerName);
105  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
106 
107  timerName = {};
108  timerTimeout = {};
109  timerCalled = false;
110 
111  // prio 30, 10 active => 30 selected
112  EXPECT_TRUE(mux.cb("o1p30", ros::Time(3.5), ros::Time(3.5)));
113 
114  EXPECT_EQ(30, mux.getActivePriority());
115  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
116  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
117  EXPECT_EQ("o1p30", mux.getLastSelectedTopics().at("o1"));
118  EXPECT_EQ("o1p30", timerName);
119  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
120 
121  timerName = {};
122  timerTimeout = {};
123  timerCalled = false;
124 
125  // prio 10, 30 active => ignored
126  EXPECT_FALSE(mux.cb("o1p10", ros::Time(3.5), ros::Time(3.5)));
127 
128  EXPECT_EQ(30, mux.getActivePriority());
129  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
130  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
131  EXPECT_EQ("o1p30", mux.getLastSelectedTopics().at("o1"));
132  EXPECT_EQ("o1p10", timerName);
133  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
134 
135  timerName = {}; timerTimeout = {}; timerCalled = false;
136 
137  // prio 20, 30 active => ignored
138  EXPECT_FALSE(mux.cb("o1p20", ros::Time(4), ros::Time(4)));
139 
140  EXPECT_EQ(30, mux.getActivePriority());
141  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
142  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
143  EXPECT_EQ("o1p30", mux.getLastSelectedTopics().at("o1"));
144  EXPECT_EQ("o1p20", timerName);
145  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
146 
147  timerName = {};
148  timerTimeout = {};
149  timerCalled = false;
150 
151  // prio 30 timed out, 20 active => 20 selected
152  mux.update(ros::Time(4.6));
153 
154  EXPECT_EQ(20, mux.getActivePriority());
155  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
156  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
157  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
158  EXPECT_FALSE(timerCalled);
159 
160  // prio 20, 20 active => 20 selected
161  EXPECT_TRUE(mux.cb("o1p20", ros::Time(5), ros::Time(5)));
162 
163  EXPECT_EQ(20, mux.getActivePriority());
164  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
165  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
166  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
167  EXPECT_EQ("o1p20", timerName);
168  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
169 
170  timerName = {};
171  timerTimeout = {};
172  timerCalled = false;
173 
174  // prio 20 timed out, none active => none selected
175  mux.update(ros::Time(6));
176 
177  EXPECT_EQ(0, mux.getActivePriority());
178  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
179  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
180  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
181  EXPECT_FALSE(timerCalled);
182 }
183 
184 TEST(PriorityMuxBase, OneOutputLongTimeout) // NOLINT
185 {
186  auto log = std::make_shared<cras::MemoryLogHelper>();
187 
188  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
189  {"o1p10", {"o1p10", "o1p10", "o1", 10, {10, 0}}},
190  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
191  };
192 
193  std::string timerName;
194  ros::Duration timerTimeout;
195  bool timerCalled {false};
196  auto setTimer = [&](const std::string& name, const ros::Duration& timeout)
197  {
198  timerName = name;
199  timerTimeout = timeout;
200  timerCalled = true;
201  };
202 
203  PriorityMux mux(topicConfigs, {}, setTimer, ros::Time(0), log);
204 
205  // prio 10, none is active => prio 10 selected
206  EXPECT_TRUE(mux.cb("o1p10", ros::Time(1), ros::Time(1)));
207 
208  EXPECT_EQ(10, mux.getActivePriority());
209  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
210  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
211  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
212  EXPECT_EQ("o1p10", timerName);
213  EXPECT_EQ(ros::Duration(10, 0), timerTimeout);
214 
215  timerName = {};
216  timerTimeout = {};
217  timerCalled = false;
218 
219  // prio 20, 10 active => prio 20 selected
220  EXPECT_TRUE(mux.cb("o1p20", ros::Time(2), ros::Time(2)));
221 
222  EXPECT_EQ(20, mux.getActivePriority());
223  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
224  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
225  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
226  EXPECT_EQ("o1p20", timerName);
227  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
228 
229  timerName = {};
230  timerTimeout = {};
231  timerCalled = false;
232 
233  // 20 active => 20 selected
234  mux.update(ros::Time(2.5));
235 
236  EXPECT_EQ(20, mux.getActivePriority());
237  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
238  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
239  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
240  EXPECT_FALSE(timerCalled);
241 
242  // 20 timed out, 10 still active => 10 selected
243  mux.update(ros::Time(3.5));
244 
245  EXPECT_EQ(10, mux.getActivePriority());
246  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
247  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
248  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
249  EXPECT_FALSE(timerCalled);
250 
251  // prio 20, 20 active => 20 selected
252  EXPECT_TRUE(mux.cb("o1p20", ros::Time(5), ros::Time(5)));
253 
254  EXPECT_EQ(20, mux.getActivePriority());
255  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
256  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
257  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
258  EXPECT_EQ("o1p20", timerName);
259  EXPECT_EQ(ros::Duration(1, 0), timerTimeout);
260 
261  timerName = {};
262  timerTimeout = {};
263  timerCalled = false;
264 
265  // prio 20 timed out, 10 still active => 10 selected
266  mux.update(ros::Time(6));
267 
268  EXPECT_EQ(10, mux.getActivePriority());
269  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
270  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
271  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
272  EXPECT_FALSE(timerCalled);
273 
274  // prio 10 is explicitly released => none selected
275  mux.disableCb("o1p10", ros::Time(7), true, ros::Time(7));
276 
277  EXPECT_EQ(0, mux.getActivePriority());
278  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
279  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
280  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
281  EXPECT_EQ("__disable_o1p10", timerName);
282  EXPECT_EQ(ros::Duration(10, 0), timerTimeout);
283 
284  timerName = {}; timerTimeout = {}; timerCalled = false;
285 
286  // prio 10 timed out, none other active => none selected
287  mux.update(ros::Time(11));
288 
289  EXPECT_EQ(0, mux.getActivePriority());
290  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
291  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
292  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
293  EXPECT_FALSE(timerCalled);
294 }
295 
296 TEST(PriorityMuxBase, DelayedMessages) // NOLINT
297 {
298  auto log = std::make_shared<cras::MemoryLogHelper>();
299 
300  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
301  {"o1p10", {"o1p10", "o1p10", "o1", 10, {10, 0}}},
302  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
303  };
304 
305  std::string timerName;
306  ros::Duration timerTimeout;
307  bool timerCalled {false};
308  auto setTimer = [&](const std::string& name, const ros::Duration& timeout)
309  {
310  timerName = name;
311  timerTimeout = timeout;
312  timerCalled = true;
313  };
314 
315  PriorityMux mux(topicConfigs, {}, setTimer, ros::Time(0), log);
316 
317  const ros::Duration halfSec(0, 500000000);
318 
319  // message came before timeout, timer will be set
320  EXPECT_TRUE(mux.cb("o1p10", ros::Time(1), ros::Time(1) + halfSec));
321 
322  EXPECT_EQ(10, mux.getActivePriority());
323  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
324  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
325  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
326  EXPECT_EQ("o1p10", timerName);
327  EXPECT_EQ(ros::Duration(9) + halfSec, timerTimeout);
328 
329  timerName = {};
330  timerTimeout = {};
331  timerCalled = false;
332 
333  // message came too late, no timer will be set and priority is unchanged
334  EXPECT_TRUE(mux.cb("o1p20", ros::Time(1) + halfSec, ros::Time(2) + halfSec));
335 
336  EXPECT_EQ(10, mux.getActivePriority());
337  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
338  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
339  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
340  EXPECT_FALSE(timerCalled);
341 }
342 
343 TEST(PriorityMuxBase, OneOutputAndLock) // NOLINT
344 {
345  auto log = std::make_shared<cras::MemoryLogHelper>();
346 
347  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
348  {"o1p10", {"o1p10", "o1p10", "o1", 10, {10, 0}}},
349  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
350  };
351 
352  std::unordered_map<std::string, cras::priority_mux::LockConfig> lockConfigs = {
353  {"l15", {"l15", "l15", 15, {1, 0}}},
354  {"l17", {"l17", "l17", 17, {0, 0}}},
355  };
356 
357  std::string timerName;
358  ros::Duration timerTimeout;
359  bool timerCalled {false};
360  auto setTimer = [&](const std::string& name, const ros::Duration& timeout)
361  {
362  timerName = name;
363  timerTimeout = timeout;
364  timerCalled = true;
365  };
366 
367  PriorityMux mux(topicConfigs, lockConfigs, setTimer, ros::Time(0), log);
368 
369  // prio 10, lock 15 locked (because never published)
370  EXPECT_FALSE(mux.cb("o1p10", ros::Time(2), ros::Time(2)));
371 
372  EXPECT_EQ(15, mux.getActivePriority());
373  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
374  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
375  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
376  EXPECT_EQ("o1p10", timerName);
377  EXPECT_EQ(ros::Duration(10), timerTimeout);
378 
379  timerName = {};
380  timerTimeout = {};
381  timerCalled = false;
382 
383  // unlock lock 15 => prio 10 selected
384  mux.lockCb("l15", ros::Time(2), false, ros::Time(2));
385 
386  EXPECT_EQ(10, mux.getActivePriority());
387  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
388  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
389  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
390  EXPECT_EQ("l15", timerName);
391  EXPECT_EQ(ros::Duration(1), timerTimeout);
392 
393  timerName = {};
394  timerTimeout = {};
395  timerCalled = false;
396 
397  // lock 15
398  mux.lockCb("l15", ros::Time(2.1), true, ros::Time(2.1));
399 
400  EXPECT_EQ(15, mux.getActivePriority());
401  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
402  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
403  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
404  EXPECT_FALSE(timerCalled); // timer not called because the lock is explicitly locked
405 
406  // prio 20, lock 15 locked => 20 selected
407  EXPECT_TRUE(mux.cb("o1p20", ros::Time(2.2), ros::Time(2.2)));
408 
409  EXPECT_EQ(20, mux.getActivePriority());
410  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
411  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
412  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
413  EXPECT_EQ("o1p20", timerName);
414  EXPECT_EQ(ros::Duration(1), timerTimeout);
415 
416  timerName = {};
417  timerTimeout = {};
418  timerCalled = false;
419 
420  // unlock lock 15 => prio 20 still selected
421  mux.lockCb("l15", ros::Time(2.7), false, ros::Time(2.7));
422 
423  EXPECT_EQ(20, mux.getActivePriority());
424  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
425  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
426  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
427  EXPECT_EQ("l15", timerName);
428  EXPECT_EQ(ros::Duration(1), timerTimeout);
429 
430  timerName = {};
431  timerTimeout = {};
432  timerCalled = false;
433 
434  // lock 17 => prio 20 still selected
435  mux.lockCb("l17", ros::Time(2.8), true, ros::Time(2.8));
436 
437  EXPECT_EQ(20, mux.getActivePriority());
438  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
439  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
440  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
441  EXPECT_FALSE(timerCalled); // timer not called because lock 17 has no timeout
442 
443  // prio 20 timed out, lock 17 locked
444  mux.update(ros::Time(3.3));
445 
446  EXPECT_EQ(17, mux.getActivePriority());
447  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
448  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
449  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
450  EXPECT_FALSE(timerCalled);
451 
452  // unlock 17 => lock 15 still unlocked, prio 10 still selected
453  mux.lockCb("l17", ros::Time(3.4), false, ros::Time(3.4));
454 
455  EXPECT_EQ(10, mux.getActivePriority());
456  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
457  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
458  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
459  EXPECT_FALSE(timerCalled); // timer not called because lock 17 has no timeout
460 
461  // lock 15 times out
462  mux.update(ros::Time(5.0));
463 
464  EXPECT_EQ(15, mux.getActivePriority());
465  ASSERT_EQ(1u, mux.getLastSelectedTopics().size());
466  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
467  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
468  EXPECT_FALSE(timerCalled);
469 }
470 
471 TEST(PriorityMuxBase, MultiOutputs) // NOLINT
472 {
473  auto log = std::make_shared<cras::MemoryLogHelper>();
474 
475  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
476  {"o1p10", {"o1p10", "o1p10", "o1", 10, {10, 0}}},
477  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
478  {"o2p10", {"o2p10", "o2p10", "o2", 10, {1, 0}}},
479  {"o2p20", {"o2p20", "o2p20", "o2", 20, {1, 0}}},
480  {"o2p30", {"o2p30", "o2p30", "o2", 30, {1, 0}}},
481  {"o3p10", {"o3p10", "o3p10", "o3", 10, {10, 0}}},
482  {"o3p20", {"o3p20", "o3p20", "o3", 20, {10, 0}}},
483  {"o3p30", {"o3p30", "o3p30", "o3", 30, {10, 0}}},
484  };
485 
486  PriorityMux mux(topicConfigs, {}, [](const std::string&, const ros::Duration&) {}, ros::Time(0), log);
487  ASSERT_EQ(3u, mux.getLastSelectedTopics().size());
488  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
489  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o2"));
490  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o3"));
491 
492  // prio 10
493  EXPECT_TRUE(mux.cb("o1p10", ros::Time(1), ros::Time(1)));
494  EXPECT_EQ(10, mux.getActivePriority());
495  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
496  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
497  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
498 
499  // prio 30
500  EXPECT_TRUE(mux.cb("o2p30", ros::Time(2), ros::Time(2)));
501  EXPECT_EQ(30, mux.getActivePriority());
502  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
503  EXPECT_EQ("o2p30", mux.getLastSelectedTopics().at("o2"));
504  EXPECT_EQ("o3p30", mux.getLastSelectedTopics().at("o3"));
505 
506  // prio 20, but 30 is active
507  EXPECT_FALSE(mux.cb("o2p20", ros::Time(2.1), ros::Time(2.1)));
508  EXPECT_EQ(30, mux.getActivePriority());
509  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
510  EXPECT_EQ("o2p30", mux.getLastSelectedTopics().at("o2"));
511  EXPECT_EQ("o3p30", mux.getLastSelectedTopics().at("o3"));
512 
513  // prio 30 times out, but 20 not yet
514  mux.update(ros::Time(3));
515  EXPECT_EQ(20, mux.getActivePriority());
516  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
517  EXPECT_EQ("o2p20", mux.getLastSelectedTopics().at("o2"));
518  EXPECT_EQ("o3p20", mux.getLastSelectedTopics().at("o3"));
519 
520  // prio 20 times out, but 10 not yet
521  mux.update(ros::Time(3.2));
522  EXPECT_EQ(10, mux.getActivePriority());
523  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
524  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
525  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
526 
527  // prio 10 times out
528  mux.update(ros::Time(12));
529  EXPECT_EQ(0, mux.getActivePriority());
530  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
531  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
532  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
533 }
534 
535 
536 TEST(PriorityMuxBase, MultiOutputsWithLocks) // NOLINT
537 {
538  auto log = std::make_shared<cras::MemoryLogHelper>();
539 
540  std::unordered_map<std::string, cras::priority_mux::TopicConfig> topicConfigs = {
541  {"o1p10", {"o1p10", "o1p10", "o1", 10, {10, 0}}},
542  {"o1p20", {"o1p20", "o1p20", "o1", 20, {1, 0}}},
543  {"o2p10", {"o2p10", "o2p10", "o2", 10, {1, 0}}},
544  {"o2p20", {"o2p20", "o2p20", "o2", 20, {1, 0}}},
545  {"o2p30", {"o2p30", "o2p30", "o2", 30, {1, 0}}},
546  {"o3p10", {"o3p10", "o3p10", "o3", 10, {10, 0}}},
547  {"o3p20", {"o3p20", "o3p20", "o3", 20, {10, 0}}},
548  {"o3p30", {"o3p30", "o3p30", "o3", 30, {10, 0}}},
549  };
550 
551  std::unordered_map<std::string, cras::priority_mux::LockConfig> lockConfigs = {
552  {"l15", {"l15", "l15", 15, {1, 0}}},
553  {"l50", {"l50", "l50", 50, {0, 0}}},
554  };
555 
556  PriorityMux mux(topicConfigs, lockConfigs, [](const std::string&, const ros::Duration&) {}, ros::Time(0), log);
557  ASSERT_EQ(3u, mux.getLastSelectedTopics().size());
558  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o1"));
559  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o2"));
560  ASSERT_NE(mux.getLastSelectedTopics().end(), mux.getLastSelectedTopics().find("o3"));
561 
562  // prio 10, lock 15 locked (because never published)
563  EXPECT_FALSE(mux.cb("o1p10", ros::Time(2), ros::Time(2)));
564 
565  EXPECT_EQ(15, mux.getActivePriority());
566  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
567  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
568  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
569 
570  // lock 15 unlocked, prio 10 becomes active
571  mux.lockCb("l15", ros::Time(2), false, ros::Time(2));
572  EXPECT_EQ(10, mux.getActivePriority());
573  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
574  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
575  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
576 
577  // prio 30, lock 15 locks
578  EXPECT_TRUE(mux.cb("o2p30", ros::Time(3), ros::Time(3)));
579  EXPECT_EQ(30, mux.getActivePriority());
580  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
581  EXPECT_EQ("o2p30", mux.getLastSelectedTopics().at("o2"));
582  EXPECT_EQ("o3p30", mux.getLastSelectedTopics().at("o3"));
583 
584  // prio 20, but 30 is active
585  EXPECT_FALSE(mux.cb("o2p20", ros::Time(3.1), ros::Time(3.1)));
586  EXPECT_EQ(30, mux.getActivePriority());
587  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
588  EXPECT_EQ("o2p30", mux.getLastSelectedTopics().at("o2"));
589  EXPECT_EQ("o3p30", mux.getLastSelectedTopics().at("o3"));
590 
591  // lock 50 locked
592  mux.lockCb("l50", ros::Time(3), true, ros::Time(3));
593  EXPECT_EQ(50, mux.getActivePriority());
594  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
595  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
596  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
597 
598  // prio 30 times out, but 20 not yet; lock 50 still locked
599  mux.update(ros::Time(4));
600  EXPECT_EQ(50, mux.getActivePriority());
601  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
602  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
603  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
604 
605  // lock 50 unlocked; prio 30 times out, but 20 not yet
606  mux.lockCb("l50", ros::Time(4), false, ros::Time(4));
607  EXPECT_EQ(20, mux.getActivePriority());
608  EXPECT_EQ("o1p20", mux.getLastSelectedTopics().at("o1"));
609  EXPECT_EQ("o2p20", mux.getLastSelectedTopics().at("o2"));
610  EXPECT_EQ("o3p20", mux.getLastSelectedTopics().at("o3"));
611 
612  // prio 20 times out, lock 15 is locked
613  mux.update(ros::Time(4.2));
614  EXPECT_EQ(15, mux.getActivePriority());
615  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
616  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
617  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
618 
619  // lock 15 is unlocked, prio 10 still not timed out
620  mux.lockCb("l15", ros::Time(5), false, ros::Time(5));
621  EXPECT_EQ(10, mux.getActivePriority());
622  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
623  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
624  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
625 
626  // try releasing a topic that is active but is not responsible for it
627  mux.disableCb("o2p10", ros::Time(5.5), true, ros::Time(5.5));
628  EXPECT_EQ(10, mux.getActivePriority());
629  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
630  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
631  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
632 
633  // release o1p10, explicitly releasing prio 10
634  mux.disableCb("o1p10", ros::Time(5.5), true, ros::Time(5.5));
635  EXPECT_EQ(0, mux.getActivePriority());
636  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
637  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
638  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
639 
640  // test that publishing to a disabled topic is blocked
641  EXPECT_FALSE(mux.cb("o1p10", ros::Time(5.55), ros::Time(5.55)));
642  EXPECT_EQ(0, mux.getActivePriority());
643  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
644  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
645  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
646 
647  // enable o1p10 again, allowing prio 10 again
648  mux.disableCb("o1p10", ros::Time(5.6), false, ros::Time(5.6));
649  EXPECT_EQ(10, mux.getActivePriority());
650  EXPECT_EQ("o1p10", mux.getLastSelectedTopics().at("o1"));
651  EXPECT_EQ("o2p10", mux.getLastSelectedTopics().at("o2"));
652  EXPECT_EQ("o3p10", mux.getLastSelectedTopics().at("o3"));
653 
654  // prio 10 times out, lock 15 is locked
655  mux.update(ros::Time(12));
656  EXPECT_EQ(15, mux.getActivePriority());
657  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
658  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
659  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
660 
661  // prio 10 times out, lock 15 unlocked
662  mux.lockCb("l15", ros::Time(16), false, ros::Time(16));
663  EXPECT_EQ(0, mux.getActivePriority());
664  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o1"));
665  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o2"));
666  EXPECT_EQ("__none", mux.getLastSelectedTopics().at("o3"));
667 }
668 
669 int main(int argc, char** argv)
670 {
671  testing::InitGoogleTest(&argc, argv);
672  return RUN_ALL_TESTS();
673 }
cras::PriorityMux::cb
virtual bool cb(const ::std::string &inTopic, const ::ros::Time &stamp, const ::ros::Time &now)
Callback function run when input topic message is received.
Definition: priority_mux_base.cpp:45
cras
time.h
cras::PriorityMux
A class for priority-based muxing of topics.
Definition: priority_mux_base.h:83
duration.h
priority_mux_base.h
Priority-based muxer for topics.
cras::PriorityMux::getActivePriority
int getActivePriority() const
Return the active priority level.
Definition: priority_mux_base.cpp:222
main
int main(int argc, char **argv)
Definition: test_priority_mux_base.cpp:669
cras::PriorityMux::lockCb
virtual void lockCb(const ::std::string &topic, const ::ros::Time &stamp, bool locked, const ::ros::Time &now)
Callback function run when a lock message is received.
Definition: priority_mux_base.cpp:112
ros::Time
memory.h
TEST
TEST(PriorityMuxBase, OneOutput)
Definition: test_priority_mux_base.cpp:24
cras::PriorityMux::getLastSelectedTopics
const ::std::unordered_map<::std::string, ::std::string > & getLastSelectedTopics() const
Return the last selected topic for each output topic.
Definition: priority_mux_base.cpp:217
cras::PriorityMux::update
virtual void update(const ::ros::Time &now)
Updates the mux after changes have been made to some of its internal state.
Definition: priority_mux_base.cpp:138
ros::Duration

cras_topic_tools
Author(s): Martin Pecka
autogenerated on Sun Mar 2 2025 03:51:09