1use std::sync::atomic::{AtomicBool, Ordering};
4use std::sync::{Arc, Mutex};
5use std::time::Duration;
6
7use anyhow::Context;
8use moqros::ros::r2r;
9use tokio::task::JoinHandle;
10
11pub fn init_logging() {
12 tracing_subscriber::fmt()
13 .with_env_filter(
14 tracing_subscriber::EnvFilter::try_from_default_env()
15 .unwrap_or_else(|_| tracing_subscriber::EnvFilter::new("info")),
16 )
17 .init();
18}
19
20pub struct SharedNode {
22 node: Mutex<r2r::Node>,
23 waiting: AtomicBool,
26}
27
28impl SharedNode {
29 pub fn new(node: r2r::Node) -> Arc<Self> {
30 Arc::new(Self {
31 node: Mutex::new(node),
32 waiting: AtomicBool::new(false),
33 })
34 }
35
36 pub fn with<R>(&self, f: impl FnOnce(&mut r2r::Node) -> R) -> R {
38 self.waiting.store(true, Ordering::Relaxed);
39 let mut node = self.node.lock().unwrap_or_else(|err| err.into_inner());
40 self.waiting.store(false, Ordering::Relaxed);
41 f(&mut node)
42 }
43}
44
45pub async fn shutdown_signal() -> anyhow::Result<&'static str> {
48 #[cfg(unix)]
49 {
50 use tokio::signal::unix::{SignalKind, signal};
51 let mut term = signal(SignalKind::terminate()).context("failed to listen for SIGTERM")?;
52 tokio::select! {
53 res = tokio::signal::ctrl_c() => res.map(|()| "SIGINT").context("failed to listen for SIGINT"),
54 _ = term.recv() => Ok("SIGTERM"),
55 }
56 }
57 #[cfg(not(unix))]
58 {
59 tokio::signal::ctrl_c().await.context("failed to listen for Ctrl-C")?;
60 Ok("Ctrl-C")
61 }
62}
63
64pub fn spawn_spinner(node: Arc<SharedNode>) -> Spinner {
69 let stop = Arc::new(AtomicBool::new(false));
70 let stopped = stop.clone();
71 let task = tokio::task::spawn_blocking(move || {
72 while !stopped.load(Ordering::Relaxed) {
73 if node.waiting.load(Ordering::Relaxed) {
74 std::thread::yield_now();
75 continue;
76 }
77 let mut guard = node.node.lock().unwrap_or_else(|err| err.into_inner());
78 guard.spin_once(Duration::from_millis(50));
79 }
80 });
81 Spinner { stop, task: Some(task) }
82}
83
84pub struct Spinner {
86 stop: Arc<AtomicBool>,
87 task: Option<JoinHandle<()>>,
88}
89
90impl Spinner {
91 pub fn idle() -> Self {
94 Self {
95 stop: Arc::new(AtomicBool::new(true)),
96 task: None,
97 }
98 }
99
100 pub async fn failed(&mut self) -> anyhow::Error {
103 let Some(task) = self.task.as_mut() else {
104 return std::future::pending().await;
105 };
106 let result = task.await;
107 self.task = None;
108 match result {
109 Err(err) => anyhow::Error::new(err).context("ROS spinner failed"),
110 Ok(()) => anyhow::anyhow!("ROS spinner stopped"),
111 }
112 }
113}
114
115impl Drop for Spinner {
116 fn drop(&mut self) {
117 self.stop.store(true, Ordering::Relaxed);
118 }
119}