1use crate::events::{Command, Event, EventKind, Meta, ZoneRange};
22use cumulus3d_core::io::ply::PointCloud;
23use std::collections::{HashMap, VecDeque};
24use std::panic::{catch_unwind, AssertUnwindSafe};
25use std::sync::atomic::{AtomicUsize, Ordering};
26use std::sync::mpsc::{channel, Receiver, Sender};
27use std::sync::{Arc, Condvar, Mutex};
28use std::thread::JoinHandle;
29use std::time::SystemTime;
30
31pub trait Reducer {
37 type Input;
39 fn step(&mut self, input: Self::Input) -> Vec<Event>;
41 fn poll(&mut self) -> Vec<Event>;
43 fn command(&mut self, c: Command) -> Vec<Event>;
45 fn finish(&mut self) -> Vec<Event> {
48 self.poll()
49 }
50}
51
52#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
54pub enum QueuePolicy {
55 #[default]
57 KeepAll,
58 LatestPerKey,
61 LatestOnly,
63}
64
65#[derive(Clone, Debug, Default)]
67pub struct Summary {
68 pub events: usize,
70 pub by_kind: HashMap<EventKind, usize>,
72 pub hook_calls: usize,
74 pub hook_panics: usize,
76 pub superseded: usize,
78}
79
80type Hook = Box<dyn FnMut(&Event) + Send + 'static>;
81type HookList = Arc<Mutex<Vec<Hook>>>;
82
83type LaneKey = Option<EventKind>;
85
86struct Shared {
88 subscribers: Mutex<Vec<Sender<Event>>>,
89 hook_calls: AtomicUsize,
90 hook_panics: AtomicUsize,
91 superseded: AtomicUsize,
92}
93
94impl Shared {
95 fn broadcast(&self, ev: &Event) {
96 let mut subs = self.subscribers.lock().unwrap_or_else(|p| p.into_inner());
97 subs.retain(|tx| tx.send(ev.clone()).is_ok());
98 }
99
100 fn run_hooks(&self, hooks: &HookList, ev: &Event) {
102 let mut hooks = hooks.lock().unwrap_or_else(|p| p.into_inner());
103 for hook in hooks.iter_mut() {
104 self.hook_calls.fetch_add(1, Ordering::Relaxed);
105 let r = catch_unwind(AssertUnwindSafe(|| hook(ev)));
106 if let Err(payload) = r {
107 self.hook_panics.fetch_add(1, Ordering::Relaxed);
108 let what = if let Some(s) = payload.downcast_ref::<&str>() {
109 (*s).to_string()
110 } else if let Some(s) = payload.downcast_ref::<String>() {
111 s.clone()
112 } else {
113 "알 수 없는 패닉".to_string()
114 };
115 let err = Event::Error {
116 meta: Meta { version: ev.meta().version, at: SystemTime::now() },
117 message: format!("{:?} 훅 패닉: {what}", ev.kind()),
118 };
119 self.broadcast(&err);
120 }
121 }
122 }
123}
124
125struct LaneQueue {
126 items: VecDeque<Event>,
127 busy: bool,
128 closed: bool,
129}
130
131struct LaneState {
132 q: Mutex<LaneQueue>,
133 cv: Condvar,
134}
135
136struct Lane {
137 hooks: HookList,
138 state: Arc<LaneState>,
139 worker: Option<JoinHandle<()>>,
140}
141
142impl Lane {
143 fn new() -> Self {
144 Lane {
145 hooks: Arc::new(Mutex::new(Vec::new())),
146 state: Arc::new(LaneState {
147 q: Mutex::new(LaneQueue { items: VecDeque::new(), busy: false, closed: false }),
148 cv: Condvar::new(),
149 }),
150 worker: None,
151 }
152 }
153
154 fn ensure_worker(&mut self, name: String, shared: &Arc<Shared>) {
155 if self.worker.is_some() {
156 return;
157 }
158 let state = Arc::clone(&self.state);
159 let hooks = Arc::clone(&self.hooks);
160 let shared = Arc::clone(shared);
161 let h = std::thread::Builder::new()
162 .name(name)
163 .spawn(move || loop {
164 let ev = {
165 let mut q = state.q.lock().unwrap_or_else(|p| p.into_inner());
166 loop {
167 if let Some(ev) = q.items.pop_front() {
168 q.busy = true;
169 break Some(ev);
170 }
171 if q.closed {
172 break None;
173 }
174 q = state.cv.wait(q).unwrap_or_else(|p| p.into_inner());
175 }
176 };
177 let Some(ev) = ev else { return };
178 shared.run_hooks(&hooks, &ev);
179 let mut q = state.q.lock().unwrap_or_else(|p| p.into_inner());
180 q.busy = false;
181 state.cv.notify_all();
182 })
183 .expect("훅 워커 스레드 생성 실패");
184 self.worker = Some(h);
185 }
186
187 fn enqueue(&self, ev: Event, policy: QueuePolicy) -> usize {
189 let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
190 let before = q.items.len();
191 match policy {
192 QueuePolicy::KeepAll => {}
193 QueuePolicy::LatestOnly => {
194 let k = ev.kind();
195 q.items.retain(|e| e.kind() != k);
196 }
197 QueuePolicy::LatestPerKey => {
198 let k = (ev.kind(), supersede_key(&ev));
199 q.items.retain(|e| (e.kind(), supersede_key(e)) != k);
200 }
201 }
202 let dropped = before - q.items.len();
203 q.items.push_back(ev);
204 self.state.cv.notify_all();
205 dropped
206 }
207
208 fn wait_idle(&self) {
210 if self.worker.is_none() {
211 return;
212 }
213 let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
214 while !q.items.is_empty() || q.busy {
215 q = self.state.cv.wait(q).unwrap_or_else(|p| p.into_inner());
216 }
217 }
218
219 fn shutdown(&mut self) {
220 {
221 let mut q = self.state.q.lock().unwrap_or_else(|p| p.into_inner());
222 q.closed = true;
223 self.state.cv.notify_all();
224 }
225 if let Some(h) = self.worker.take() {
226 let _ = h.join();
227 }
228 }
229}
230
231fn supersede_key(ev: &Event) -> Option<usize> {
233 match ev {
234 Event::ZoneArrived { range, .. }
235 | Event::ZonePreview { range, .. }
236 | Event::ZoneRefinedPose { range, .. }
237 | Event::ZoneRefined { range, .. } => Some(range.zone),
238 Event::BaseAdopted { zone, .. }
239 | Event::Reanchored { zone, .. }
240 | Event::RefineStarted { zone, .. }
241 | Event::ZoneAdjusted { zone, .. }
242 | Event::ZoneInvalidated { zone, .. } => Some(*zone),
243 Event::Snapshot { index, .. } => Some(*index),
244 Event::PositionDone { position, .. } | Event::FrameIngested { position, .. } => Some(*position),
245 _ => None,
246 }
247}
248
249struct Lanes {
251 map: HashMap<LaneKey, Lane>,
252}
253
254impl Lanes {
255 fn shutdown(&mut self) {
256 for lane in self.map.values_mut() {
257 lane.shutdown();
258 }
259 }
260}
261
262impl Drop for Lanes {
263 fn drop(&mut self) {
264 self.shutdown();
265 }
266}
267
268pub struct Pipeline<R: Reducer> {
270 reducer: R,
271 lanes: Lanes,
272 policies: HashMap<EventKind, QueuePolicy>,
273 shared: Arc<Shared>,
274 sync: bool,
275 events: usize,
276 by_kind: HashMap<EventKind, usize>,
277}
278
279impl<R: Reducer> Pipeline<R> {
280 pub fn new(reducer: R) -> Self {
282 Pipeline {
283 reducer,
284 lanes: Lanes { map: HashMap::new() },
285 policies: HashMap::new(),
286 shared: Arc::new(Shared {
287 subscribers: Mutex::new(Vec::new()),
288 hook_calls: AtomicUsize::new(0),
289 hook_panics: AtomicUsize::new(0),
290 superseded: AtomicUsize::new(0),
291 }),
292 sync: false,
293 events: 0,
294 by_kind: HashMap::new(),
295 }
296 }
297
298 pub fn sync(mut self, on: bool) -> Self {
300 if on && !self.sync {
301 self.flush();
302 }
303 self.sync = on;
304 self
305 }
306
307 pub fn policy(mut self, kind: EventKind, p: QueuePolicy) -> Self {
309 self.policies.insert(kind, p);
310 self
311 }
312
313 pub fn on<F>(mut self, kind: EventKind, f: F) -> Self
315 where
316 F: FnMut(&Event) + Send + 'static,
317 {
318 self.add_hook(Some(kind), Box::new(f));
319 self
320 }
321
322 pub fn on_any<F>(mut self, f: F) -> Self
324 where
325 F: FnMut(&Event) + Send + 'static,
326 {
327 self.add_hook(None, Box::new(f));
328 self
329 }
330
331 pub fn on_zone_preview<F>(self, mut f: F) -> Self
333 where
334 F: FnMut(ZoneRange, &Arc<PointCloud>) + Send + 'static,
335 {
336 self.on(EventKind::ZonePreview, move |e| {
337 if let Event::ZonePreview { range, cloud, .. } = e {
338 f(*range, cloud)
339 }
340 })
341 }
342
343 pub fn on_zone_refined<F>(self, mut f: F) -> Self
345 where
346 F: FnMut(ZoneRange, &Arc<PointCloud>) + Send + 'static,
347 {
348 self.on(EventKind::ZoneRefined, move |e| {
349 if let Event::ZoneRefined { range, cloud, .. } = e {
350 f(*range, cloud)
351 }
352 })
353 }
354
355 pub fn on_snapshot<F>(self, mut f: F) -> Self
357 where
358 F: FnMut(usize, &Arc<PointCloud>) + Send + 'static,
359 {
360 self.on(EventKind::Snapshot, move |e| {
361 if let Event::Snapshot { index, cloud, .. } = e {
362 f(*index, cloud)
363 }
364 })
365 }
366
367 pub fn on_position_done<F>(self, mut f: F) -> Self
369 where
370 F: FnMut(usize, usize, usize) + Send + 'static,
371 {
372 self.on(EventKind::PositionDone, move |e| {
373 if let Event::PositionDone { position, registered, expected, .. } = e {
374 f(*position, *registered, *expected)
375 }
376 })
377 }
378
379 pub fn on_message<F>(self, f: F) -> Self
381 where
382 F: FnMut(EventKind, &str) + Send + 'static,
383 {
384 let f = Arc::new(Mutex::new(f));
385 let g = Arc::clone(&f);
386 self.on(EventKind::Warning, move |e| {
387 if let Event::Warning { message, .. } = e {
388 (f.lock().unwrap_or_else(|p| p.into_inner()))(EventKind::Warning, message)
389 }
390 })
391 .on(EventKind::Error, move |e| {
392 if let Event::Error { message, .. } = e {
393 (g.lock().unwrap_or_else(|p| p.into_inner()))(EventKind::Error, message)
394 }
395 })
396 }
397
398 fn add_hook(&mut self, key: LaneKey, hook: Hook) {
399 let lane = self.lanes.map.entry(key).or_insert_with(Lane::new);
400 lane.hooks.lock().unwrap_or_else(|p| p.into_inner()).push(hook);
401 }
402
403 pub fn subscribe(&self) -> Receiver<Event> {
405 let (tx, rx) = channel();
406 self.shared.subscribers.lock().unwrap_or_else(|p| p.into_inner()).push(tx);
407 rx
408 }
409
410 pub fn push(&mut self, input: R::Input) -> usize {
412 let evs = self.reducer.step(input);
413 self.dispatch(evs)
414 }
415
416 pub fn poll(&mut self) -> usize {
418 let evs = self.reducer.poll();
419 self.dispatch(evs)
420 }
421
422 pub fn command(&mut self, c: Command) -> usize {
424 let evs = self.reducer.command(c);
425 self.dispatch(evs)
426 }
427
428 pub fn flush(&mut self) {
430 for lane in self.lanes.map.values() {
431 lane.wait_idle();
432 }
433 }
434
435 pub fn reducer(&self) -> &R {
437 &self.reducer
438 }
439
440 pub fn reducer_mut(&mut self) -> &mut R {
442 &mut self.reducer
443 }
444
445 pub fn finish(mut self) -> (R, Summary) {
448 let evs = self.reducer.finish();
449 self.dispatch(evs);
450 self.lanes.shutdown();
451 self.shared.subscribers.lock().unwrap_or_else(|p| p.into_inner()).clear();
452 let summary = Summary {
453 events: self.events,
454 by_kind: std::mem::take(&mut self.by_kind),
455 hook_calls: self.shared.hook_calls.load(Ordering::Relaxed),
456 hook_panics: self.shared.hook_panics.load(Ordering::Relaxed),
457 superseded: self.shared.superseded.load(Ordering::Relaxed),
458 };
459 let Pipeline { reducer, .. } = self;
460 (reducer, summary)
461 }
462
463 fn dispatch(&mut self, evs: Vec<Event>) -> usize {
464 let n = evs.len();
465 for ev in evs {
466 self.events += 1;
467 *self.by_kind.entry(ev.kind()).or_insert(0) += 1;
468 self.shared.broadcast(&ev);
469 let kind = ev.kind();
470 if self.sync {
471 for key in [Some(kind), None] {
472 if let Some(lane) = self.lanes.map.get(&key) {
473 self.shared.run_hooks(&lane.hooks, &ev);
474 }
475 }
476 continue;
477 }
478 let policy = self.policies.get(&kind).copied().unwrap_or_default();
479 let has_kind = self.lanes.map.contains_key(&Some(kind));
480 let has_any = self.lanes.map.contains_key(&None);
481 let mut pending = Some(ev);
482 for (key, present, last) in [(Some(kind), has_kind, !has_any), (None, has_any, true)] {
483 if !present {
484 continue;
485 }
486 let ev = if last { pending.take() } else { pending.clone() };
487 let Some(ev) = ev else { break };
488 let name = match key {
489 Some(k) => format!("hook-{k:?}"),
490 None => "hook-any".to_string(),
491 };
492 let lane = self.lanes.map.get_mut(&key).expect("레인 존재 확인됨");
493 lane.ensure_worker(name, &self.shared);
494 let dropped = lane.enqueue(ev, policy);
495 self.shared.superseded.fetch_add(dropped, Ordering::Relaxed);
496 }
497 }
498 n
499 }
500}
501
502#[cfg(test)]
503mod tests {
504 use super::*;
505 use cumulus3d_core::Reconstruction;
506 use std::sync::mpsc::RecvTimeoutError;
507 use std::time::Duration;
508
509 fn meta(v: u64) -> Meta {
510 Meta { version: v, at: SystemTime::now() }
511 }
512 fn range(z: usize) -> ZoneRange {
513 ZoneRange { zone: z, lo: z * 10, hi: z * 10 + 10 }
514 }
515 fn cloud() -> Arc<PointCloud> {
516 Arc::new(PointCloud::default())
517 }
518 fn preview(v: u64, z: usize) -> Event {
519 Event::ZonePreview {
520 meta: meta(v),
521 range: range(z),
522 cloud: cloud(),
523 model: Arc::new(Reconstruction::default()),
524 dense: true,
525 frame: "gps".to_string(),
526 }
527 }
528 fn refined(v: u64, z: usize) -> Event {
529 Event::ZoneRefined {
530 meta: meta(v),
531 range: range(z),
532 cloud: cloud(),
533 model: Arc::new(Reconstruction::default()),
534 dense: true,
535 }
536 }
537
538 #[derive(Default)]
541 struct Fake {
542 v: u64,
543 zones: Vec<usize>,
544 commands: Vec<String>,
545 }
546
547 impl Reducer for Fake {
548 type Input = usize;
549 fn step(&mut self, n: usize) -> Vec<Event> {
550 self.v += 1;
551 let mut out = vec![Event::PositionDone { meta: meta(self.v), position: n, registered: n, expected: n }];
552 if n.is_multiple_of(2) {
553 out.push(preview(self.v, n / 2));
554 self.zones.push(n / 2);
555 }
556 out
557 }
558 fn poll(&mut self) -> Vec<Event> {
559 Vec::new()
560 }
561 fn command(&mut self, c: Command) -> Vec<Event> {
562 self.commands.push(format!("{c:?}"));
563 match c {
564 Command::InvalidateZone(z) => vec![Event::Warning { meta: meta(self.v), message: format!("invalidate {z}") }],
565 Command::ResetFrom(p) => vec![Event::Warning { meta: meta(self.v), message: format!("reset {p}") }],
566 }
567 }
568 fn finish(&mut self) -> Vec<Event> {
569 self.zones
570 .drain(..)
571 .map(|z| refined(self.v, z))
572 .collect()
573 }
574 }
575
576 fn log<T>() -> Arc<Mutex<Vec<T>>> {
577 Arc::new(Mutex::new(Vec::new()))
578 }
579
580 #[test]
581 fn pipeline_sync_hook_order() {
582 let l = log();
583 let (a, b, c) = (Arc::clone(&l), Arc::clone(&l), Arc::clone(&l));
584 let mut p = Pipeline::new(Fake::default())
585 .sync(true)
586 .on_any(move |e| a.lock().unwrap().push(format!("any:{:?}", e.kind())))
587 .on_position_done(move |pos, _, _| b.lock().unwrap().push(format!("pos:{pos}")))
588 .on_zone_preview(move |r, _| c.lock().unwrap().push(format!("prev:{}", r.zone)));
589 p.push(1);
590 p.push(2);
591 let (_, s) = p.finish();
592 let got = l.lock().unwrap().clone();
593 assert_eq!(
594 got,
595 vec![
596 "pos:1", "any:PositionDone", "pos:2", "any:PositionDone", "prev:1", "any:ZonePreview", "any:ZoneRefined"
597 ]
598 );
599 assert_eq!(s.events, 4);
600 assert_eq!(s.by_kind[&EventKind::PositionDone], 2);
601 }
602
603 #[test]
604 fn pipeline_async_order_within_kind() {
605 let l = log::<String>();
606 let any = log::<String>();
607 let (a, b) = (Arc::clone(&l), Arc::clone(&any));
608 let mut p = Pipeline::new(Fake::default())
609 .on_position_done(move |pos, _, _| {
610 if pos.is_multiple_of(7) {
611 std::thread::sleep(Duration::from_millis(1));
612 }
613 a.lock().unwrap().push(pos.to_string())
614 })
615 .on_any(move |e| b.lock().unwrap().push(format!("{:?}", e.kind())));
616 for i in 0..200 {
617 p.push(i);
618 }
619 let (_, s) = p.finish();
620 let got: Vec<usize> = l.lock().unwrap().iter().map(|s| s.parse().unwrap()).collect();
621 assert_eq!(got, (0..200).collect::<Vec<_>>());
622 assert_eq!(any.lock().unwrap().len(), s.events);
623 assert_eq!(s.superseded, 0);
624 }
625
626 #[test]
627 fn pipeline_latest_per_key_policy() {
628 let gate = Arc::new((Mutex::new(false), Condvar::new()));
630 let seen = log();
631 let (g, s2) = (Arc::clone(&gate), Arc::clone(&seen));
632
633 struct Previews(u64);
634 impl Reducer for Previews {
635 type Input = (usize, u64);
636 fn step(&mut self, (z, v): (usize, u64)) -> Vec<Event> {
637 self.0 += 1;
638 vec![preview(v, z)]
639 }
640 fn poll(&mut self) -> Vec<Event> {
641 Vec::new()
642 }
643 fn command(&mut self, _: Command) -> Vec<Event> {
644 Vec::new()
645 }
646 }
647
648 let mut p = Pipeline::new(Previews(0)).policy(EventKind::ZonePreview, QueuePolicy::LatestPerKey).on(
649 EventKind::ZonePreview,
650 move |e| {
651 let (m, cv) = &*g;
652 let mut open = m.lock().unwrap();
653 while !*open {
654 open = cv.wait(open).unwrap();
655 }
656 if let Event::ZonePreview { meta, range, .. } = e {
657 s2.lock().unwrap().push(format!("{}@{}", range.zone, meta.version));
658 }
659 },
660 );
661 p.push((9, 0));
663 std::thread::sleep(Duration::from_millis(50));
664 for v in 1..=5 {
665 p.push((1, v));
666 p.push((2, v));
667 }
668 {
669 let (m, cv) = &*gate;
670 *m.lock().unwrap() = true;
671 cv.notify_all();
672 }
673 let (_, s) = p.finish();
674 assert_eq!(seen.lock().unwrap().clone(), vec!["9@0", "1@5", "2@5"]);
675 assert_eq!(s.superseded, 8);
676 assert_eq!(s.events, 11);
677 }
678
679 #[test]
680 fn pipeline_panic_isolated() {
681 let l = log();
682 let a = Arc::clone(&l);
683 let mut p = Pipeline::new(Fake::default())
684 .on_position_done(|pos, _, _| {
685 if pos == 1 {
686 panic!("boom");
687 }
688 })
689 .on_position_done(move |pos, _, _| a.lock().unwrap().push(pos));
690 let rx = p.subscribe();
691 for i in 0..4 {
692 p.push(i);
693 }
694 let (_, s) = p.finish();
695 assert_eq!(l.lock().unwrap().clone(), vec![0, 1, 2, 3]);
696 assert_eq!(s.hook_panics, 1);
697 let errs: Vec<String> = rx
698 .iter()
699 .filter_map(|e| match e {
700 Event::Error { message, meta } => {
701 assert_eq!(meta.version, 2);
702 Some(message)
703 }
704 _ => None,
705 })
706 .collect();
707 assert_eq!(errs.len(), 1);
708 assert!(errs[0].contains("boom"), "{}", errs[0]);
709 }
710
711 #[test]
712 fn pipeline_channel_receives_all() {
713 let mut p = Pipeline::new(Fake::default());
714 let rx = p.subscribe();
715 p.push(1);
716 p.push(2);
717 let kinds: Vec<EventKind> = rx.try_iter().map(|e| e.kind()).collect();
718 assert_eq!(kinds, vec![EventKind::PositionDone, EventKind::PositionDone, EventKind::ZonePreview]);
719 let dropped = p.subscribe();
720 drop(dropped);
721 p.push(3);
722 let (_, s) = p.finish();
723 let rest: Vec<EventKind> = rx.iter().map(|e| e.kind()).collect();
724 assert_eq!(rest, vec![EventKind::PositionDone, EventKind::ZoneRefined]);
725 assert_eq!(s.events, 5);
726 assert!(matches!(rx.recv_timeout(Duration::from_millis(1)), Err(RecvTimeoutError::Disconnected)));
727 }
728
729 #[test]
730 fn pipeline_finish_drains() {
731 let n = Arc::new(AtomicUsize::new(0));
732 let refined = log();
733 let (a, b) = (Arc::clone(&n), Arc::clone(&refined));
734 let mut p = Pipeline::new(Fake::default())
735 .on(EventKind::PositionDone, move |_| {
736 std::thread::sleep(Duration::from_micros(200));
737 a.fetch_add(1, Ordering::SeqCst);
738 })
739 .on_zone_refined(move |r, _| b.lock().unwrap().push(r.zone));
740 for i in 0..50 {
741 p.push(i);
742 }
743 let (r, s) = p.finish();
744 assert_eq!(n.load(Ordering::SeqCst), 50);
745 assert_eq!(refined.lock().unwrap().len(), 25);
746 assert!(r.zones.is_empty());
747 assert_eq!(s.hook_calls, 75);
748 }
749
750 #[test]
751 fn pipeline_command_forwarded() {
752 let msgs = log();
753 let a = Arc::clone(&msgs);
754 let mut p = Pipeline::new(Fake::default()).sync(true).on_message(move |k, m| a.lock().unwrap().push(format!("{k:?}:{m}")));
755 assert_eq!(p.command(Command::InvalidateZone(3)), 1);
756 p.command(Command::ResetFrom(7));
757 assert_eq!(p.reducer().commands, vec!["InvalidateZone(3)", "ResetFrom(7)"]);
758 let (_, _) = p.finish();
759 assert_eq!(msgs.lock().unwrap().clone(), vec!["Warning:invalidate 3", "Warning:reset 7"]);
760 }
761}