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