chalkydri/
main.rs

1//!
2//! Chalkydri core
3//!
4
5// Unsafe code is NOT allowed in Chalkydri core.
6// If unsafe code is required, it should be part of a different crate.
7#![forbid(unsafe_code)]
8#![allow(unreachable_code)]
9
10#[macro_use]
11extern crate log;
12//extern crate actix_web;
13extern crate env_logger;
14extern crate minint;
15#[cfg(feature = "mjpeg")]
16extern crate mozjpeg;
17extern crate tokio;
18//extern crate utoipa as utopia;
19#[macro_use]
20extern crate serde;
21#[cfg(feature = "capriltags")]
22extern crate apriltag;
23#[cfg(feature = "apriltags")]
24extern crate chalkydri_apriltags;
25#[cfg(feature = "python")]
26extern crate pyo3;
27#[cfg(feature = "ml")]
28extern crate tfledge;
29
30//mod api;
31mod calibration;
32mod cameras;
33mod config;
34mod error;
35mod logger;
36mod subsys;
37mod subsystem;
38mod utils;
39
40//use api::run_api;
41use cameras::load_cameras;
42use config::Config;
43use logger::Logger;
44use mimalloc::MiMalloc;
45use minint::NtConn;
46use once_cell::sync::Lazy;
47#[cfg(feature = "rerun")]
48use re_sdk::{MemoryLimit, RecordingStream};
49#[cfg(feature = "rerun_web_viewer")]
50use re_web_viewer_server::WebViewerServerPort;
51#[cfg(feature = "rerun")]
52use re_ws_comms::RerunServerPort;
53use std::{error::Error, path::Path, sync::Arc, time::Duration};
54#[cfg(feature = "capriltags")]
55use subsys::capriltags::CApriltagsDetector;
56use tokio::{
57    sync::{watch, RwLock},
58    task::LocalSet,
59};
60
61// mimalloc is a very good general purpose allocator
62#[global_allocator]
63static GLOBAL: MiMalloc = MiMalloc;
64
65use utils::gen_team_ip;
66
67use subsystem::Subsystem;
68
69#[allow(non_upper_case_globals)]
70static Cfg: Lazy<RwLock<Config>> = Lazy::new(|| {
71    let mut path = Path::new("/boot/chalkydri.toml");
72    if !path.exists() {
73        path = Path::new("/etc/chalkydri.toml");
74        if !path.exists() {
75            path = Path::new("./chalkydri.toml");
76        }
77    }
78
79    RwLock::new(Config::load(path).unwrap())
80});
81
82#[cfg(feature = "rerun")]
83#[allow(non_upper_case_globals)]
84static Rerun: Lazy<RecordingStream> = Lazy::new(|| {
85    #[cfg(feature = "rerun_web_viewer")]
86    re_sdk::RecordingStreamBuilder::new("chalkydri")
87        .serve_web(
88            "0.0.0.0",
89            WebViewerServerPort(8080),
90            RerunServerPort(6969),
91            MemoryLimit::from_bytes(10_000_000),
92            true,
93        )
94        .unwrap()
95        .into()
96});
97
98#[tokio::main(worker_threads = 16)]
99async fn main() -> Result<(), Box<dyn Error>> {
100    Logger::new().with_path_prefix("logs/handler").init()?;
101
102    info!("Chalkydri starting up...");
103
104    let roborio_ip = gen_team_ip(Cfg.read().await.team_number).expect("failed to generate team ip");
105    // Generate a random device id
106    let dev_id = fastrand::u32(..);
107
108    // Attempt to connect to the NT server, retrying until successful
109
110    let nt: NtConn;
111
112    let mut retry = false;
113
114    loop {
115        match NtConn::new(roborio_ip, format!("chalkydri{dev_id}")).await {
116            Ok(conn) => {
117                nt = conn;
118                break;
119            }
120            Err(err) => {
121                if !retry {
122                    error!("Error connecting to NT server: {err:?}");
123                    retry = true;
124                }
125                tokio::time::sleep(Duration::from_millis(5)).await;
126            }
127        }
128    }
129
130    info!("Connected to NT server at {roborio_ip:?} successfully!");
131
132    // Create a channel for sharing frames from the camera thread with the subsystems
133    let (tx, mut rx) = watch::channel::<Arc<Vec<u8>>>(Arc::new(Vec::new()));
134
135    // Spawn a thread to handle cameras
136    std::thread::spawn(move || {
137        let tx = tx.clone();
138        load_cameras(tx).unwrap();
139    });
140
141    // apriltag C library subsystem
142    let local = LocalSet::new();
143    let nt_ = nt.clone();
144    local
145        .spawn_local(async move {
146            let nt = nt_;
147
148            // Initialize the apriltag C library subsystem
149            let mut at = CApriltagsDetector::init().await.unwrap();
150
151            // Publish NT topics
152            let mut translation = nt
153                .publish::<Vec<f64>>(&format!("/chalkydri/robot_pose/translation"))
154                .await
155                .unwrap();
156            let mut rotation = nt
157                .publish::<Vec<f64>>(&format!("/chalkydri/robot_pose/rotation"))
158                .await
159                .unwrap();
160            let mut timestamp = nt
161                .publish::<String>(&format!("/chalkydri/robot_pose/timestamp"))
162                .await
163                .unwrap();
164
165            loop {
166                // Wait for a new image from the camera
167                if rx.changed().await.is_ok() {
168                    // Get timestamp for the image
169                    let ts = chrono::Utc::now().to_rfc3339();
170                    // Borrow the buffer and let the channel know we've seen this value
171                    let buf = rx.borrow_and_update();
172
173                    // Make a copy of the buffer and release the borrow of the original
174                    let buf_ = buf.clone();
175                    drop(buf);
176
177                    // Send the buffer to AprilTag detector
178                    let pose = at.process(buf_).unwrap();
179
180                    // Unpack the pose into translation and rotation
181                    let (t, r) = pose;
182
183                    // Update the translation, rotation, and timestamp on NetworkTables
184                    translation.set(t.clone()).await.unwrap();
185                    rotation.set(r.clone()).await.unwrap();
186                    timestamp.set(ts).await.unwrap();
187                }
188            }
189        })
190        .await
191        .unwrap();
192
193    // Have to let NT topics get dropped before calling nt.stop()
194    {
195        //run_api(nt.clone()).await;
196    }
197
198    // Shut down NT connection
199    nt.stop();
200
201    Ok(())
202}
chalkydri/main.rs

chalkydri/
main.rs
