From 74fb43e8f48d6c106db0efdad52f88705400e69f Mon Sep 17 00:00:00 2001 From: Manglemix Date: Sun, 25 Feb 2024 16:01:17 -0700 Subject: [PATCH] added main macro --- Cargo.lock | 40 ++- Cargo.toml | 2 +- apriltag/Cargo.toml | 2 +- apriltag/examples/webcam.rs | 61 ++-- apriltag/src/lib.rs | 8 +- camera/Cargo.toml | 2 +- camera/src/lib.rs | 2 +- camera/src/main.rs | 53 ++-- costmap/Cargo.toml | 2 +- costmap/src/lib.rs | 2 +- drive/Cargo.toml | 2 +- drive/src/lib.rs | 2 +- localization/Cargo.toml | 2 +- localization/src/lib.rs | 2 +- lunabot/Cargo.toml | 2 +- lunabot/src/main.rs | 354 +++++++++++----------- lunasimbot/Cargo.toml | 2 +- lunasimbot/src/main.rs | 515 +++++++++++++++----------------- navigator/Cargo.toml | 2 +- navigator/src/lib.rs | 2 +- navigator/src/pathfinders.rs | 10 +- realsense/Cargo.toml | 2 +- realsense/src/implementation.rs | 10 +- realsense/src/iter.rs | 4 +- realsense/src/main.rs | 54 ++-- serial/Cargo.toml | 2 +- serial/src/lib.rs | 2 +- serial/src/main.rs | 75 +++-- smooth-diff-drive/Cargo.toml | 2 +- smooth-diff-drive/src/drive.rs | 7 +- telemetry/Cargo.toml | 2 +- telemetry/src/lib.rs | 2 +- telemetry/src/main.rs | 28 +- unros-core/src/lib.rs | 5 +- unros-core/src/logging/dump.rs | 14 +- unros-core/src/logging/mod.rs | 4 +- unros-core/src/service/mod.rs | 83 +++-- unros-macros/Cargo.toml | 12 + unros-macros/src/lib.rs | 47 +++ unros/Cargo.toml | 10 + unros/src/lib.rs | 2 + 41 files changed, 745 insertions(+), 691 deletions(-) create mode 100644 unros-macros/Cargo.toml create mode 100644 unros-macros/src/lib.rs create mode 100644 unros/Cargo.toml create mode 100644 unros/src/lib.rs diff --git a/Cargo.lock b/Cargo.lock index 93e7e8a4..14124276 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -154,7 +154,7 @@ dependencies = [ "nalgebra", "rig", "toml", - "unros-core", + "unros", ] [[package]] @@ -809,7 +809,7 @@ version = "0.1.0" dependencies = [ "image", "nokhwa", - "unros-core", + "unros", ] [[package]] @@ -1239,7 +1239,7 @@ dependencies = [ "nalgebra", "ordered-float", "spin_sleep", - "unros-core", + "unros", ] [[package]] @@ -1532,7 +1532,7 @@ version = "0.1.0" dependencies = [ "global-msgs", "serial", - "unros-core", + "unros", ] [[package]] @@ -2752,7 +2752,7 @@ dependencies = [ "rand_distr", "rig", "smach", - "unros-core", + "unros", ] [[package]] @@ -2786,7 +2786,7 @@ dependencies = [ "rig", "telemetry", "toml", - "unros-core", + "unros", ] [[package]] @@ -2800,7 +2800,7 @@ dependencies = [ "navigator", "rig", "toml", - "unros-core", + "unros", ] [[package]] @@ -3078,7 +3078,7 @@ dependencies = [ "pathfinding", "rig", "spin_sleep", - "unros-core", + "unros", ] [[package]] @@ -3892,7 +3892,7 @@ dependencies = [ "quaternion-core", "realsense-rust", "rig", - "unros-core", + "unros", ] [[package]] @@ -4297,7 +4297,7 @@ name = "serial" version = "0.1.0" dependencies = [ "tokio-serial", - "unros-core", + "unros", "vesc-comm", ] @@ -4453,7 +4453,7 @@ dependencies = [ "rand_distr", "serde", "serde-big-array", - "unros-core", + "unros", ] [[package]] @@ -4677,7 +4677,7 @@ dependencies = [ "num_enum", "ordered-float", "spin_sleep", - "unros-core", + "unros", ] [[package]] @@ -5185,6 +5185,14 @@ version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" +[[package]] +name = "unros" +version = "0.1.0" +dependencies = [ + "unros-core", + "unros-macros", +] + [[package]] name = "unros-core" version = "0.1.0" @@ -5210,6 +5218,14 @@ dependencies = [ "tokio-rayon", ] +[[package]] +name = "unros-macros" +version = "0.1.0" +dependencies = [ + "quote", + "syn 2.0.50", +] + [[package]] name = "untrusted" version = "0.9.0" diff --git a/Cargo.toml b/Cargo.toml index 743190b1..dd056921 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ members = [ "navigator", "rig", "tools", "smach", "costmap", "smooth-diff-drive", - "lunasimbot", + "lunasimbot", "unros-macros", "unros", ] resolver = "2" diff --git a/apriltag/Cargo.toml b/apriltag/Cargo.toml index a713f264..3ffef191 100644 --- a/apriltag/Cargo.toml +++ b/apriltag/Cargo.toml @@ -9,7 +9,7 @@ edition = "2021" apriltag-inner = { version = "0.4", package = "apriltag" } apriltag-image = "0.1" apriltag-nalgebra = "0.1" -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } rig = { path = "../rig" } nalgebra = { workspace = true } fxhash = { workspace = true } diff --git a/apriltag/examples/webcam.rs b/apriltag/examples/webcam.rs index 47dc05c6..154dd330 100644 --- a/apriltag/examples/webcam.rs +++ b/apriltag/examples/webcam.rs @@ -2,42 +2,37 @@ use apriltag::AprilTagDetector; use camera::discover_all_cameras; use fxhash::FxBuildHasher; use rig::Robot; -use unros_core::{ +use unros::{ anyhow::{self, Context}, - default_run_options, pubsub::Subscriber, - start_unros_runtime, + Application, }; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - |mut app| async { - let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; - let (mut elements, _) = rig.destructure::(["camera"])?; - let camera_element = elements.remove("camera").unwrap(); - let mut apriltag = AprilTagDetector::new(1108.4, 1280, 960, camera_element.get_ref()); - let mut camera = discover_all_cameras() - .context("Failed to discover cameras")? - .next() - .context("No camera found")?; - camera.accept_image_received_sub(apriltag.create_image_subscription()); - let mut pose_sub = Subscriber::new(1); - apriltag.add_tag(Default::default(), Default::default(), 0.12, 0); - apriltag.accept_tag_detected_sub(pose_sub.create_subscription()); - app.add_task( - |_| async move { - loop { - let pose = pose_sub.recv().await; - println!("{pose}"); - } - }, - "pose", - ); - - app.add_node(camera); - app.add_node(apriltag); - Ok(app) +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; + let (mut elements, _) = rig.destructure::(["camera"])?; + let camera_element = elements.remove("camera").unwrap(); + let mut apriltag = AprilTagDetector::new(1108.4, 1280, 960, camera_element.get_ref()); + let mut camera = discover_all_cameras() + .context("Failed to discover cameras")? + .next() + .context("No camera found")?; + camera.accept_image_received_sub(apriltag.create_image_subscription()); + let mut pose_sub = Subscriber::new(1); + apriltag.add_tag(Default::default(), Default::default(), 0.12, 0); + apriltag.accept_tag_detected_sub(pose_sub.create_subscription()); + app.add_task( + |_| async move { + loop { + let pose = pose_sub.recv().await; + println!("{pose}"); + } }, - default_run_options!(), - ) + "pose", + ); + + app.add_node(camera); + app.add_node(apriltag); + Ok(app) } diff --git a/apriltag/src/lib.rs b/apriltag/src/lib.rs index 11441ae4..bfe95f36 100644 --- a/apriltag/src/lib.rs +++ b/apriltag/src/lib.rs @@ -14,7 +14,7 @@ use apriltag_nalgebra::PoseExt; use fxhash::FxHashMap; use nalgebra::{Isometry3, Point3, UnitQuaternion, Vector3}; use rig::RobotElementRef; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Publisher, Subscriber, Subscription}, setup_logging, @@ -46,11 +46,7 @@ impl Debug for PoseObservation { impl Display for PoseObservation { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - let axis = self - .pose - .rotation - .axis() - .unwrap_or_else(Vector3::x_axis); + let axis = self.pose.rotation.axis().unwrap_or_else(Vector3::x_axis); write!(f, "Observer pos: ({:.2}, {:.2}, {:.2}), axis: ({:.2}, {:.2}, {:.2}), angle: {:.0}, margin: {:.0}", self.pose.translation.x, self.pose.translation.y, self.pose.translation.z, axis.x, axis.y, axis.z, self.pose.rotation.angle() / PI * 180.0, self.decision_margin) // write!(f, "Observer pos: ({:.2}, {:.2}, {:.2}), quat: ({:.2}, {:.2}, {:.2}, {:.2}), margin: {:.0}", self.pose.translation.x, self.pose.translation.y, self.pose.translation.z, self.pose.rotation.w, self.pose.rotation.i, self.pose.rotation.j, self.pose.rotation.k, self.decision_margin) } diff --git a/camera/Cargo.toml b/camera/Cargo.toml index d6079c9c..8f841d44 100644 --- a/camera/Cargo.toml +++ b/camera/Cargo.toml @@ -7,5 +7,5 @@ edition = "2021" [dependencies] nokhwa = { version = "0.10", features = ["input-native"] } -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } image = { workspace = true } \ No newline at end of file diff --git a/camera/src/lib.rs b/camera/src/lib.rs index 451125d2..5c4828b4 100644 --- a/camera/src/lib.rs +++ b/camera/src/lib.rs @@ -12,7 +12,7 @@ use nokhwa::{ query, utils::{CameraIndex, RequestedFormat, RequestedFormatType}, }; -use unros_core::{ +use unros::{ anyhow::{self, Context}, async_trait, log, pubsub::{Publisher, Subscription}, diff --git a/camera/src/main.rs b/camera/src/main.rs index 9403a4a9..7d61d064 100644 --- a/camera/src/main.rs +++ b/camera/src/main.rs @@ -1,39 +1,34 @@ use std::sync::atomic::{AtomicUsize, Ordering}; use camera::discover_all_cameras; -use unros_core::{ +use unros::{ anyhow::{self, Context}, - default_run_options, log::info, pubsub::Subscriber, - start_unros_runtime, tokio, + tokio, Application, }; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - |mut app| async { - let fps = 20; - discover_all_cameras() - .context("Failed to discover cameras")? - .for_each(|mut camera| { - let mut sub = Subscriber::new(8); - camera.accept_image_received_sub(sub.create_subscription()); - let frame_count = AtomicUsize::new(0); - let idx = camera.camera_index; - tokio::spawn(async move { - loop { - sub.recv().await; - let frame_count = frame_count.fetch_add(1, Ordering::Relaxed) + 1; - if frame_count % (fps as usize) == 0 { - info!("idx: {idx} -> {frame_count}"); - } - } - }); - app.add_node(camera); - }); +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + let fps = 20; + discover_all_cameras() + .context("Failed to discover cameras")? + .for_each(|mut camera| { + let mut sub = Subscriber::new(8); + camera.accept_image_received_sub(sub.create_subscription()); + let frame_count = AtomicUsize::new(0); + let idx = camera.camera_index; + tokio::spawn(async move { + loop { + sub.recv().await; + let frame_count = frame_count.fetch_add(1, Ordering::Relaxed) + 1; + if frame_count % (fps as usize) == 0 { + info!("idx: {idx} -> {frame_count}"); + } + } + }); + app.add_node(camera); + }); - Ok(app) - }, - default_run_options!(), - ) + Ok(app) } diff --git a/costmap/Cargo.toml b/costmap/Cargo.toml index b5744b0a..cc514a8b 100644 --- a/costmap/Cargo.toml +++ b/costmap/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } nalgebra = { workspace = true } image = { workspace = true, optional = false } spin_sleep = { workspace = true } diff --git a/costmap/src/lib.rs b/costmap/src/lib.rs index 4e42980e..3db877d6 100644 --- a/costmap/src/lib.rs +++ b/costmap/src/lib.rs @@ -14,7 +14,7 @@ use std::{ use nalgebra::{Dyn, Matrix, Point2, Point3, VecStorage}; use ordered_float::NotNan; use spin_sleep::SpinSleeper; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Subscriber, Subscription}, rayon::{ diff --git a/drive/Cargo.toml b/drive/Cargo.toml index 1d1ea3e6..6fcfc249 100644 --- a/drive/Cargo.toml +++ b/drive/Cargo.toml @@ -6,6 +6,6 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } serial = { path = "../serial" } global-msgs = { path = "../global-msgs" } \ No newline at end of file diff --git a/drive/src/lib.rs b/drive/src/lib.rs index a8a901eb..5bac654f 100644 --- a/drive/src/lib.rs +++ b/drive/src/lib.rs @@ -1,6 +1,6 @@ use global_msgs::Steering; use serial::SerialConnection; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Publisher, Subscriber, Subscription}, setup_logging, tokio, Node, NodeIntrinsics, RuntimeContext, diff --git a/localization/Cargo.toml b/localization/Cargo.toml index 719fbad0..68864ff1 100644 --- a/localization/Cargo.toml +++ b/localization/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } nalgebra = { workspace = true } rig = { path = "../rig" } # quaternion-core = { workspace = true } diff --git a/localization/src/lib.rs b/localization/src/lib.rs index 1c5b23b8..be15829b 100644 --- a/localization/src/lib.rs +++ b/localization/src/lib.rs @@ -20,7 +20,7 @@ use rand::Rng; use rand_distr::{Distribution, Normal}; use rig::{RobotBase, RobotElementRef}; use smach::{start_machine, Transition}; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Subscriber, Subscription}, rayon::{ diff --git a/lunabot/Cargo.toml b/lunabot/Cargo.toml index ddef95a5..4974fdc3 100644 --- a/lunabot/Cargo.toml +++ b/lunabot/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } telemetry = { path = "../telemetry" } realsense = { path = "../realsense" } camera = { path = "../camera" } diff --git a/lunabot/src/main.rs b/lunabot/src/main.rs index b6ca3741..85ae8819 100644 --- a/lunabot/src/main.rs +++ b/lunabot/src/main.rs @@ -16,198 +16,186 @@ use navigator::{pathfinders::DirectPathfinder, DifferentialDriver}; use realsense::{discover_all_realsense, PointCloud}; use rig::Robot; use telemetry::Telemetry; -use unros_core::{ - anyhow, default_run_options, +use unros::{ + anyhow, log::info, logging::dump::{DataDump, ScalingFilter, VideoDataDump}, pubsub::Subscriber, - start_unros_runtime, tokio, Node, + tokio, Application, Node, }; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - |mut app| async { - let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; - let (mut elements, robot_base) = rig.destructure::(["camera"])?; - let camera_element = elements.remove("camera").unwrap(); - let robot_base_ref = robot_base.get_ref(); - - let costmap = Costmap::new(80, 80, 0.05, 2.0, 3.9, 0.01); - - #[cfg(unix)] - let costmap = costmap; - - let camera_count = discover_all_cameras()? - .map(|mut x| x.get_intrinsics().ignore_drop()) - .count(); - info!("Discovered {camera_count} cameras"); - let mut camera = Camera::new(0)?; - - #[cfg(unix)] - let mut realsense_camera = { - use unros_core::rayon::iter::ParallelIterator; - let mut camera = discover_all_realsense()? - .next() - .ok_or_else(|| anyhow::anyhow!("No realsense camera"))?; - - camera.set_robot_element_ref(camera_element.get_ref()); - camera.accept_cloud_received_sub( - costmap - .create_points_sub() - .map(|x: PointCloud| x.par_iter().map(|x| x.0)), - ); - - camera - }; - - let telemetry = Telemetry::new( - SocketAddrV4::from_str("10.8.0.6:43721").unwrap(), - 1920, - 1200, - 24, - ) - .await?; - camera.accept_image_received_sub(telemetry.create_image_subscription()); - - let costmap_ref = costmap.get_ref(); - - let mut costmap_writer = VideoDataDump::new_file( - 80, - 80, - 720, - 720, - ScalingFilter::Neighbor, - "costmap.mkv", - 24, - )?; - // let mut subtitle_writer = costmap_writer.init_subtitles().await?; - - app.add_task( - |_| async move { - loop { - tokio::time::sleep(Duration::from_millis(42)).await; - let costmap = costmap_ref.get_costmap(); - let obstacles = costmap_ref.costmap_to_obstacle(&costmap, 0.5, 0.0, 0.0); - let img = costmap_ref.obstacles_to_img(&obstacles); - - costmap_writer.write_frame(img.into())?; - } - }, - "costmap_video", - ); - - let mut apriltag = AprilTagDetector::new(640.0, 1280, 720, camera_element.get_ref()); - apriltag.add_tag(Default::default(), Default::default(), 0.134, 0); - // let mut pc_sub = Subscriber::default(); - // camera.accept_cloud_received_sub(pc_sub.create_subscription(1)); - - // let las_node = FnNode::new(|_| async move { - // let mut i = 0; - // loop { - // let PointCloud { points } = pc_sub.recv().await; - // unros_core::tokio_rayon::spawn(move || { - // let mut header = las::Builder::default(); - // header.point_format = las::point::Format { - // has_color: true, - // ..Default::default() - // }; - // let mut writer = las::Writer::from_path(format!("{i}.las"), header.into_header().unwrap()).unwrap(); - // use las::Write; - // for (point, color) in points.iter() { - // let mut point = las::Point { x: point.x as f64, z: point.y as f64, y: point.z as f64, ..Default::default() }; - // point.color = Some(las::Color { red: color.0[0] as u16 * 255, green: color.0[1] as u16 * 255, blue: color.0[2] as u16 * 255 }); - // writer.write(point).unwrap(); - // } - // }).await; - // i += 1; - // } - // }); - - let localizer = Localizer::new(robot_base, 0.4); - - apriltag.accept_tag_detected_sub(localizer.create_position_sub().map( - |pose: PoseObservation| PositionFrame { - position: nalgebra::convert(Point3::from(pose.pose.translation.vector)), - variance: 0.1, - robot_element: pose.robot_element, - }, - )); - - apriltag.accept_tag_detected_sub(localizer.create_orientation_sub().map( - |pose: PoseObservation| OrientationFrame { - orientation: nalgebra::convert(pose.pose.rotation), - variance: 0.1, - robot_element: pose.robot_element, - }, - )); - - #[cfg(unix)] - { - realsense_camera.accept_image_received_sub( - apriltag - .create_image_subscription() - .set_name("RealSense Apriltag Image"), - ); - // camera.accept_imu_frame_received_sub(localizer.create_imu_sub().set_name("RealSense IMU")); - } +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; + let (mut elements, robot_base) = rig.destructure::(["camera"])?; + let camera_element = elements.remove("camera").unwrap(); + let robot_base_ref = robot_base.get_ref(); + + let costmap = Costmap::new(80, 80, 0.05, 2.0, 3.9, 0.01); + + #[cfg(unix)] + let costmap = costmap; + + let camera_count = discover_all_cameras()? + .map(|mut x| x.get_intrinsics().ignore_drop()) + .count(); + info!("Discovered {camera_count} cameras"); + let mut camera = Camera::new(0)?; + + #[cfg(unix)] + let mut realsense_camera = { + use unros::rayon::iter::ParallelIterator; + let mut camera = discover_all_realsense()? + .next() + .ok_or_else(|| anyhow::anyhow!("No realsense camera"))?; + + camera.set_robot_element_ref(camera_element.get_ref()); + camera.accept_cloud_received_sub( + costmap + .create_points_sub() + .map(|x: PointCloud| x.par_iter().map(|x| x.0)), + ); + + camera + }; + + let telemetry = Telemetry::new( + SocketAddrV4::from_str("10.8.0.6:43721").unwrap(), + 1920, + 1200, + 24, + ) + .await?; + camera.accept_image_received_sub(telemetry.create_image_subscription()); - let mut navigator = - DirectPathfinder::new(robot_base_ref.clone(), costmap.get_ref(), 0.5, 0.15); - let driver = DifferentialDriver::new(robot_base_ref.clone()); - - navigator.accept_path_sub(driver.create_path_sub()); - - let mut data_dump = DataDump::new_file("motion.csv").await?; - writeln!( - data_dump, - "imu_ax,imu_ay,imu_az,imu_rvw,imu_rvi,imu_rvj,imu_rvk,vx,vy,vz,x,y,z,w,i,j,k,delta" - ) - .unwrap(); - let mut imu_sub = Subscriber::::new(32); - #[cfg(unix)] - realsense_camera.accept_imu_frame_received_sub(imu_sub.create_subscription()); - app.add_task(|_| async move { - let start = Instant::now(); - let mut elapsed = Duration::ZERO; + let costmap_ref = costmap.get_ref(); + let mut costmap_writer = + VideoDataDump::new_file(80, 80, 720, 720, ScalingFilter::Neighbor, "costmap.mkv", 24)?; + // let mut subtitle_writer = costmap_writer.init_subtitles().await?; + + app.add_task( + |_| async move { loop { - let imu = imu_sub.recv().await; - let Isometry { - translation: pos, - rotation, - } = robot_base_ref.get_isometry(); - let vel = robot_base_ref.get_linear_velocity(); - let now = start.elapsed(); - writeln!( - data_dump, - "{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4}", - imu.acceleration.x, - imu.acceleration.y, - imu.acceleration.z, - imu.angular_velocity.w, - imu.angular_velocity.i, - imu.angular_velocity.j, - imu.angular_velocity.k, - vel.x, vel.y, vel.z, - pos.x, pos.y, pos.z, - rotation.w, rotation.i, rotation.j, rotation.k, - (now - elapsed).as_secs_f32() - ).unwrap(); - elapsed = now; + tokio::time::sleep(Duration::from_millis(42)).await; + let costmap = costmap_ref.get_costmap(); + let obstacles = costmap_ref.costmap_to_obstacle(&costmap, 0.5, 0.0, 0.0); + let img = costmap_ref.obstacles_to_img(&obstacles); + + costmap_writer.write_frame(img.into())?; } - }, "telemetry-dump"); - - app.add_node(apriltag); - app.add_node(localizer); - app.add_node(driver); - app.add_node(costmap); - app.add_node(navigator); - app.add_node(telemetry); - app.add_node(camera); - app.add_node(realsense_camera); - - Ok(app) }, - default_run_options!(), + "costmap_video", + ); + + let mut apriltag = AprilTagDetector::new(640.0, 1280, 720, camera_element.get_ref()); + apriltag.add_tag(Default::default(), Default::default(), 0.134, 0); + // let mut pc_sub = Subscriber::default(); + // camera.accept_cloud_received_sub(pc_sub.create_subscription(1)); + + // let las_node = FnNode::new(|_| async move { + // let mut i = 0; + // loop { + // let PointCloud { points } = pc_sub.recv().await; + // unros::tokio_rayon::spawn(move || { + // let mut header = las::Builder::default(); + // header.point_format = las::point::Format { + // has_color: true, + // ..Default::default() + // }; + // let mut writer = las::Writer::from_path(format!("{i}.las"), header.into_header().unwrap()).unwrap(); + // use las::Write; + // for (point, color) in points.iter() { + // let mut point = las::Point { x: point.x as f64, z: point.y as f64, y: point.z as f64, ..Default::default() }; + // point.color = Some(las::Color { red: color.0[0] as u16 * 255, green: color.0[1] as u16 * 255, blue: color.0[2] as u16 * 255 }); + // writer.write(point).unwrap(); + // } + // }).await; + // i += 1; + // } + // }); + + let localizer = Localizer::new(robot_base, 0.4); + + apriltag.accept_tag_detected_sub(localizer.create_position_sub().map( + |pose: PoseObservation| PositionFrame { + position: nalgebra::convert(Point3::from(pose.pose.translation.vector)), + variance: 0.1, + robot_element: pose.robot_element, + }, + )); + + apriltag.accept_tag_detected_sub(localizer.create_orientation_sub().map( + |pose: PoseObservation| OrientationFrame { + orientation: nalgebra::convert(pose.pose.rotation), + variance: 0.1, + robot_element: pose.robot_element, + }, + )); + + #[cfg(unix)] + { + realsense_camera.accept_image_received_sub( + apriltag + .create_image_subscription() + .set_name("RealSense Apriltag Image"), + ); + // camera.accept_imu_frame_received_sub(localizer.create_imu_sub().set_name("RealSense IMU")); + } + + let mut navigator = DirectPathfinder::new(robot_base_ref.clone(), costmap.get_ref(), 0.5, 0.15); + let driver = DifferentialDriver::new(robot_base_ref.clone()); + + navigator.accept_path_sub(driver.create_path_sub()); + + let mut data_dump = DataDump::new_file("motion.csv").await?; + writeln!( + data_dump, + "imu_ax,imu_ay,imu_az,imu_rvw,imu_rvi,imu_rvj,imu_rvk,vx,vy,vz,x,y,z,w,i,j,k,delta" ) + .unwrap(); + let mut imu_sub = Subscriber::::new(32); + #[cfg(unix)] + realsense_camera.accept_imu_frame_received_sub(imu_sub.create_subscription()); + app.add_task(|_| async move { + let start = Instant::now(); + let mut elapsed = Duration::ZERO; + + loop { + let imu = imu_sub.recv().await; + let Isometry { + translation: pos, + rotation, + } = robot_base_ref.get_isometry(); + let vel = robot_base_ref.get_linear_velocity(); + let now = start.elapsed(); + writeln!( + data_dump, + "{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4}", + imu.acceleration.x, + imu.acceleration.y, + imu.acceleration.z, + imu.angular_velocity.w, + imu.angular_velocity.i, + imu.angular_velocity.j, + imu.angular_velocity.k, + vel.x, vel.y, vel.z, + pos.x, pos.y, pos.z, + rotation.w, rotation.i, rotation.j, rotation.k, + (now - elapsed).as_secs_f32() + ).unwrap(); + elapsed = now; + } +}, "telemetry-dump"); + + app.add_node(apriltag); + app.add_node(localizer); + app.add_node(driver); + app.add_node(costmap); + app.add_node(navigator); + app.add_node(telemetry); + app.add_node(camera); + app.add_node(realsense_camera); + + Ok(app) } diff --git a/lunasimbot/Cargo.toml b/lunasimbot/Cargo.toml index 2b5fa0e3..b91d4edb 100644 --- a/lunasimbot/Cargo.toml +++ b/lunasimbot/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } rig = { path = "../rig" } costmap = { path = "../costmap" } fxhash = { workspace = true } diff --git a/lunasimbot/src/main.rs b/lunasimbot/src/main.rs index 03c10d40..d442b503 100644 --- a/lunasimbot/src/main.rs +++ b/lunasimbot/src/main.rs @@ -4,315 +4,288 @@ use localization::{IMUFrame, Localizer, OrientationFrame, PositionFrame, Velocit use nalgebra::{Point3, Quaternion, UnitQuaternion, Vector3}; use navigator::{pathfinders::DirectPathfinder, DifferentialDriver}; use rig::Robot; -use unros_core::{ - anyhow, default_run_options, log, +use unros::{ + anyhow, log, pubsub::{Publisher, Subscriber}, - start_unros_runtime, tokio::{ self, io::{AsyncReadExt, AsyncWriteExt, BufStream}, net::TcpListener, }, + Application, }; type Float = f32; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - |mut app| async { - let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; - let (mut elements, robot_base) = - rig.destructure::(["camera", "debug"])?; - let mut camera = elements.remove("camera").unwrap(); - let debug_element = elements.remove("debug").unwrap(); +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + let rig: Robot = toml::from_str(include_str!("lunabot.toml"))?; + let (mut elements, robot_base) = rig.destructure::(["camera", "debug"])?; + let mut camera = elements.remove("camera").unwrap(); + let debug_element = elements.remove("debug").unwrap(); - let costmap = Costmap::new(400, 400, 0.05, 10.0, 10.0, 0.01); - let mut points_signal = Publisher::>>::default(); + let costmap = Costmap::new(400, 400, 0.05, 10.0, 10.0, 0.01); + let mut points_signal = Publisher::>>::default(); - points_signal.accept_subscription(costmap.create_points_sub()); - // let costmap_ref = costmap.get_ref(); + points_signal.accept_subscription(costmap.create_points_sub()); + // let costmap_ref = costmap.get_ref(); - // let mut costmap_writer = unros_core::logging::dump::VideoDataDump::new(720, 720, "costmap.mkv")?; + // let mut costmap_writer = unros::logging::dump::VideoDataDump::new(720, 720, "costmap.mkv")?; - // let video_maker = FnNode::new(|_| async move { - // // let mut i = 0; - // loop { - // tokio::time::sleep(std::time::Duration::from_millis(42)).await; - // let costmap = costmap_ref.get_costmap(); - // let obstacles = costmap_ref.costmap_to_obstacle(&costmap, 0.2, 0.0, 0.3); - // let img = costmap_ref.obstacles_to_img(&obstacles); + // let video_maker = FnNode::new(|_| async move { + // // let mut i = 0; + // loop { + // tokio::time::sleep(std::time::Duration::from_millis(42)).await; + // let costmap = costmap_ref.get_costmap(); + // let obstacles = costmap_ref.costmap_to_obstacle(&costmap, 0.2, 0.0, 0.3); + // let img = costmap_ref.obstacles_to_img(&obstacles); - // i += 1; - // let _ = img.save(format!("img{i}.png")); + // i += 1; + // let _ = img.save(format!("img{i}.png")); - // // costmap_writer.write_frame(img.into()).unwrap(); - // } - // }); + // // costmap_writer.write_frame(img.into()).unwrap(); + // } + // }); - let mut pathfinder = - DirectPathfinder::new(robot_base.get_ref(), costmap.get_ref(), 0.65, 0.2); + let mut pathfinder = DirectPathfinder::new(robot_base.get_ref(), costmap.get_ref(), 0.65, 0.2); - let mut driver = DifferentialDriver::new(robot_base.get_ref()); - // driver.can_reverse = true; - pathfinder.accept_path_sub(driver.create_path_sub()); - let nav_task = pathfinder.get_navigation_handle(); + let mut driver = DifferentialDriver::new(robot_base.get_ref()); + // driver.can_reverse = true; + pathfinder.accept_path_sub(driver.create_path_sub()); + let nav_task = pathfinder.get_navigation_handle(); - // let robot_base_ref = robot_base.get_ref(); - let localizer = Localizer::new(robot_base, 0.0); + // let robot_base_ref = robot_base.get_ref(); + let localizer = Localizer::new(robot_base, 0.0); - let mut position_pub = Publisher::default(); - position_pub.accept_subscription(localizer.create_position_sub().set_name("position")); + let mut position_pub = Publisher::default(); + position_pub.accept_subscription(localizer.create_position_sub().set_name("position")); - let mut velocity_pub = Publisher::default(); - velocity_pub.accept_subscription(localizer.create_velocity_sub().set_name("velocity")); + let mut velocity_pub = Publisher::default(); + velocity_pub.accept_subscription(localizer.create_velocity_sub().set_name("velocity")); - let mut orientation_pub = Publisher::default(); - orientation_pub - .accept_subscription(localizer.create_orientation_sub().set_name("orientation")); + let mut orientation_pub = Publisher::default(); + orientation_pub.accept_subscription(localizer.create_orientation_sub().set_name("orientation")); - let mut imu_pub = Publisher::default(); - imu_pub.accept_subscription(localizer.create_imu_sub().set_name("imu")); + let mut imu_pub = Publisher::default(); + imu_pub.accept_subscription(localizer.create_imu_sub().set_name("imu")); - let mut steering_sub = Subscriber::new(1); - driver.accept_steering_sub(steering_sub.create_subscription()); + let mut steering_sub = Subscriber::new(1); + driver.accept_steering_sub(steering_sub.create_subscription()); - let tcp_listener = TcpListener::bind("0.0.0.0:11433").await?; - app.add_task( - |_| async move { - loop { - let (stream, _) = tcp_listener - .accept() - .await - .expect("Connection should have succeeded"); - let mut stream = BufStream::new(stream); - let mut points = vec![]; - let mut last_left_steering = 0.0; - let mut last_right_steering = 0.0; + let tcp_listener = TcpListener::bind("0.0.0.0:11433").await?; + app.add_task( + |_| async move { + loop { + let (stream, _) = tcp_listener + .accept() + .await + .expect("Connection should have succeeded"); + let mut stream = BufStream::new(stream); + let mut points = vec![]; + let mut last_left_steering = 0.0; + let mut last_right_steering = 0.0; - loop { - let x = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let _y = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let z = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vx = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let _vy = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vz = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let w = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let i = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let j = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let k = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vw = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vi = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vj = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let vk = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let x_rot = stream - .read_f32_le() - .await - .expect("Failed to receive packet") - as Float; - let n = stream - .read_u32_le() - .await - .expect("Failed to receive packet") - as usize; + loop { + let x = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let _y = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let z = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vx = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let _vy = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vz = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let w = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let i = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let j = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let k = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vw = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vi = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vj = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let vk = stream + .read_f32_le() + .await + .expect("Failed to receive packet") as Float; + let x_rot = stream + .read_f32_le() + .await + .expect("Failed to receive packet") + as Float; + let n = stream + .read_u32_le() + .await + .expect("Failed to receive packet") as usize; - position_pub.set(PositionFrame::rand( - Point3::new(x, 0.0, z), - 0.02, - debug_element.get_ref(), - )); - velocity_pub.set(VelocityFrame::rand( - Vector3::new(vx, 0.0, vz), - 0.02, - debug_element.get_ref(), - )); - orientation_pub.set(OrientationFrame::rand( - UnitQuaternion::new_unchecked(Quaternion::new(w, i, j, k)), - 0.03, - debug_element.get_ref(), - )); - imu_pub.set(IMUFrame::rand( - Vector3::new(0.0, -9.81, 0.0), - 0.0, - UnitQuaternion::new_unchecked(Quaternion::new(vw, vi, vj, vk)), - 0.03, - debug_element.get_ref(), - )); + position_pub.set(PositionFrame::rand( + Point3::new(x, 0.0, z), + 0.02, + debug_element.get_ref(), + )); + velocity_pub.set(VelocityFrame::rand( + Vector3::new(vx, 0.0, vz), + 0.02, + debug_element.get_ref(), + )); + orientation_pub.set(OrientationFrame::rand( + UnitQuaternion::new_unchecked(Quaternion::new(w, i, j, k)), + 0.03, + debug_element.get_ref(), + )); + imu_pub.set(IMUFrame::rand( + Vector3::new(0.0, -9.81, 0.0), + 0.0, + UnitQuaternion::new_unchecked(Quaternion::new(vw, vi, vj, vk)), + 0.03, + debug_element.get_ref(), + )); - let mut camera_joint = match camera.get_local_joint() { - rig::joints::JointMut::Hinge(x) => x, - _ => unreachable!(), - }; + let mut camera_joint = match camera.get_local_joint() { + rig::joints::JointMut::Hinge(x) => x, + _ => unreachable!(), + }; - camera_joint.set_angle(x_rot); - let camera_isometry = camera.get_global_isometry(); - points.reserve(n.saturating_sub(points.capacity())); - // let mut first = true; - for _ in 0..n { - let x = - stream.read_f32_le().await.expect("Failed to receive point"); - let y = - stream.read_f32_le().await.expect("Failed to receive point"); - let z = - stream.read_f32_le().await.expect("Failed to receive point"); - let point = camera_isometry * Point3::new(x, y, z); - // if first { - // println!("{point:?}"); - // first = false; - // } - points.push(point); - } + camera_joint.set_angle(x_rot); + let camera_isometry = camera.get_global_isometry(); + points.reserve(n.saturating_sub(points.capacity())); + // let mut first = true; + for _ in 0..n { + let x = stream.read_f32_le().await.expect("Failed to receive point"); + let y = stream.read_f32_le().await.expect("Failed to receive point"); + let z = stream.read_f32_le().await.expect("Failed to receive point"); + let point = camera_isometry * Point3::new(x, y, z); + // if first { + // println!("{point:?}"); + // first = false; + // } + points.push(point); + } - let capacity = points.capacity(); - points_signal.set(points); - points = Vec::with_capacity(capacity); + let capacity = points.capacity(); + points_signal.set(points); + points = Vec::with_capacity(capacity); - if stream - .read_u8() - .await - .expect("Failed to receive waypoint byte") - == 255 - { - let x = - stream.read_f32_le().await.expect("Failed to receive point"); - let y = - stream.read_f32_le().await.expect("Failed to receive point"); - match nav_task.try_schedule(Point3::new(x, 0.0, y)).await { - Ok(handle) => { - tokio::spawn(async move { - match handle.wait().await { - Ok(()) => log::info!("Navigation complete"), - Err(e) => log::error!("{e}"), - } - }); + if stream + .read_u8() + .await + .expect("Failed to receive waypoint byte") + == 255 + { + let x = stream.read_f32_le().await.expect("Failed to receive point"); + let y = stream.read_f32_le().await.expect("Failed to receive point"); + match nav_task.try_schedule(Point3::new(x, 0.0, y)).await { + Ok(handle) => { + tokio::spawn(async move { + match handle.wait().await { + Ok(()) => log::info!("Navigation complete"), + Err(e) => log::error!("{e}"), } - Err(e) => log::error!("{e}"), - } + }); } + Err(e) => log::error!("{e}"), + } + } - if let Some(steering) = steering_sub.try_recv() { - last_left_steering = steering.left.into_inner(); - last_right_steering = steering.right.into_inner(); - stream - .write_f32_le(last_left_steering) - .await - .expect("Failed to write steering"); - stream - .write_f32_le(last_right_steering) - .await - .expect("Failed to write steering"); - } else { - stream - .write_f32_le(last_left_steering) - .await - .expect("Failed to write steering"); - stream - .write_f32_le(last_right_steering) - .await - .expect("Failed to write steering"); - } + if let Some(steering) = steering_sub.try_recv() { + last_left_steering = steering.left.into_inner(); + last_right_steering = steering.right.into_inner(); + stream + .write_f32_le(last_left_steering) + .await + .expect("Failed to write steering"); + stream + .write_f32_le(last_right_steering) + .await + .expect("Failed to write steering"); + } else { + stream + .write_f32_le(last_left_steering) + .await + .expect("Failed to write steering"); + stream + .write_f32_le(last_right_steering) + .await + .expect("Failed to write steering"); + } - let isometry = camera.get_isometry_of_base(); + let isometry = camera.get_isometry_of_base(); - stream - .write_f32_le(isometry.translation.x) - .await - .expect("Failed to write position"); - stream - .write_f32_le(isometry.translation.y) - .await - .expect("Failed to write position"); - stream - .write_f32_le(isometry.translation.z) - .await - .expect("Failed to write position"); + stream + .write_f32_le(isometry.translation.x) + .await + .expect("Failed to write position"); + stream + .write_f32_le(isometry.translation.y) + .await + .expect("Failed to write position"); + stream + .write_f32_le(isometry.translation.z) + .await + .expect("Failed to write position"); - stream - .write_f32_le(isometry.rotation.w) - .await - .expect("Failed to write orientation"); - stream - .write_f32_le(isometry.rotation.i) - .await - .expect("Failed to write orientation"); - stream - .write_f32_le(isometry.rotation.j) - .await - .expect("Failed to write orientation"); - stream - .write_f32_le(isometry.rotation.k) - .await - .expect("Failed to write orientation"); + stream + .write_f32_le(isometry.rotation.w) + .await + .expect("Failed to write orientation"); + stream + .write_f32_le(isometry.rotation.i) + .await + .expect("Failed to write orientation"); + stream + .write_f32_le(isometry.rotation.j) + .await + .expect("Failed to write orientation"); + stream + .write_f32_le(isometry.rotation.k) + .await + .expect("Failed to write orientation"); - stream.flush().await.expect("Failed to write steering"); - } - } - }, - "telemetry", - ); + stream.flush().await.expect("Failed to write steering"); + } + } + }, + "telemetry", + ); - app.add_node(costmap); - app.add_node(driver); - app.add_node(pathfinder); - app.add_node(localizer); + app.add_node(costmap); + app.add_node(driver); + app.add_node(pathfinder); + app.add_node(localizer); - Ok(app) - }, - default_run_options!(), - ) + Ok(app) } diff --git a/navigator/Cargo.toml b/navigator/Cargo.toml index 6d126e22..fbaa08d9 100644 --- a/navigator/Cargo.toml +++ b/navigator/Cargo.toml @@ -7,7 +7,7 @@ edition = "2021" [dependencies] # crossbeam = { workspace = true } -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } global-msgs = { path = "../global-msgs" } rig = { path = "../rig" } nalgebra = { workspace = true } diff --git a/navigator/src/lib.rs b/navigator/src/lib.rs index a29ca5b3..8a03dc4f 100644 --- a/navigator/src/lib.rs +++ b/navigator/src/lib.rs @@ -4,7 +4,7 @@ use global_msgs::Steering; use nalgebra::{Point2, UnitVector2, Vector2}; use ordered_float::NotNan; use rig::{RigSpace, RobotBaseRef}; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Publisher, Subscriber, Subscription}, setup_logging, tokio_rayon, Node, NodeIntrinsics, RuntimeContext, diff --git a/navigator/src/pathfinders.rs b/navigator/src/pathfinders.rs index ae01dc7a..4498c8f1 100644 --- a/navigator/src/pathfinders.rs +++ b/navigator/src/pathfinders.rs @@ -11,8 +11,11 @@ use nalgebra::{DMatrix, Point2, Point3, Vector2}; use ordered_float::NotNan; use pathfinding::directed::astar::astar; use rig::RobotBaseRef; -use unros_core::{ - anyhow, async_trait, pubsub::{Publisher, Subscription}, service::{new_service, Service, ServiceHandle}, setup_logging, tokio_rayon, Node, NodeIntrinsics, RuntimeContext +use unros::{ + anyhow, async_trait, + pubsub::{Publisher, Subscription}, + service::{new_service, Service, ServiceHandle}, + setup_logging, tokio_rayon, Node, NodeIntrinsics, RuntimeContext, }; use crate::Float; @@ -49,7 +52,8 @@ pub struct DirectPathfinder { service_handle: NavigationServiceHandle, path_signal: Publisher>>, robot_base: RobotBaseRef, - service: Service, NavigationError, NavigationProgress, Result<(), NavigationError>>, + service: + Service, NavigationError, NavigationProgress, Result<(), NavigationError>>, pub completion_distance: Float, costmap_ref: CostmapRef, pub refresh_rate: Duration, diff --git a/realsense/Cargo.toml b/realsense/Cargo.toml index d450df49..8beaa775 100644 --- a/realsense/Cargo.toml +++ b/realsense/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } localization = { path = "../localization" } rig = { path = "../rig" } image = { workspace = true } diff --git a/realsense/src/implementation.rs b/realsense/src/implementation.rs index 33ef759b..8819a8ca 100644 --- a/realsense/src/implementation.rs +++ b/realsense/src/implementation.rs @@ -24,7 +24,7 @@ use realsense_rust::{ pipeline::InactivePipeline, }; use rig::RobotElementRef; -use unros_core::{ +use unros::{ anyhow, async_trait, pubsub::{Publisher, Subscription}, rayon::{ @@ -302,11 +302,9 @@ impl Node for RealSenseCamera { }), ExtrinsicParameters::from_pose(&nalgebra::convert(isometry)), ); - let pixel_coords = - (0..frame_height / 4).rev().flat_map(|y| { - (0..frame_width / 4) - .flat_map(move |x| [x as f32, y as f32]) - }); + let pixel_coords = (0..frame_height / 4).rev().flat_map(|y| { + (0..frame_width / 4).flat_map(move |x| [x as f32, y as f32]) + }); let pixel_coords = Pixels::new(Matrix::< f32, diff --git a/realsense/src/iter.rs b/realsense/src/iter.rs index 1df3e0a3..6d3f2aa3 100644 --- a/realsense/src/iter.rs +++ b/realsense/src/iter.rs @@ -1,6 +1,6 @@ use std::sync::Arc; -use unros_core::rayon::iter::{ +use unros::rayon::iter::{ plumbing::{bridge, Consumer, Producer, ProducerCallback, UnindexedConsumer}, IndexedParallelIterator, ParallelIterator, }; @@ -141,7 +141,7 @@ impl Producer for IterProducer { mod tests { use std::{collections::HashSet, sync::Arc}; - use unros_core::rayon::iter::{IntoParallelIterator, ParallelIterator}; + use unros::rayon::iter::{IntoParallelIterator, ParallelIterator}; use super::ArcIter; diff --git a/realsense/src/main.rs b/realsense/src/main.rs index 252d8866..aaf549ff 100644 --- a/realsense/src/main.rs +++ b/realsense/src/main.rs @@ -1,34 +1,30 @@ #[cfg(unix)] -fn main() -> unros_core::anyhow::Result<()> { - use unros_core::{default_run_options, pubsub::Subscriber, start_unros_runtime, tokio}; +#[unros::main] +async fn main(mut app: unros::Application) -> unros::anyhow::Result { + use unros::{pubsub::Subscriber, tokio}; - start_unros_runtime( - |mut app| async { - realsense::discover_all_realsense()?.for_each(|mut x| { - let mut img_sub = Subscriber::new(4); - x.accept_image_received_sub(img_sub.create_subscription()); - // let mut imu_sub = x.imu_frame_received().watch(); - tokio::spawn(async move { - let mut i = 0; - loop { - let img = img_sub.recv().await; - img.save(format!("{i}.png")).unwrap(); - i += 1; - // println!("{:?}", img_sub.recv().await.dimensions()); - // let imu = imu_sub.wait_for_change().await; - // println!( - // "ang_vel: {} accel: {}", - // imu.angular_velocity / PI * 180.0, - // imu.acceleration - // ); - } - }); - app.add_node(x); - }); - Ok(app) - }, - default_run_options!(), - ) + realsense::discover_all_realsense()?.for_each(|mut x| { + let mut img_sub = Subscriber::new(4); + x.accept_image_received_sub(img_sub.create_subscription()); + // let mut imu_sub = x.imu_frame_received().watch(); + tokio::spawn(async move { + let mut i = 0; + loop { + let img = img_sub.recv().await; + img.save(format!("{i}.png")).unwrap(); + i += 1; + // println!("{:?}", img_sub.recv().await.dimensions()); + // let imu = imu_sub.wait_for_change().await; + // println!( + // "ang_vel: {} accel: {}", + // imu.angular_velocity / PI * 180.0, + // imu.acceleration + // ); + } + }); + app.add_node(x); + }); + Ok(app) } #[cfg(not(unix))] diff --git a/serial/Cargo.toml b/serial/Cargo.toml index 0cd49549..a9f4ff6c 100644 --- a/serial/Cargo.toml +++ b/serial/Cargo.toml @@ -7,7 +7,7 @@ edition = "2021" [dependencies] tokio-serial = "5" -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } vesc-comm = { git = "https://github.com/manglemix/vesc-comm.git" } # tokio-util = { version = "0.7", features = ["codec"] } # futures = "0.3" diff --git a/serial/src/lib.rs b/serial/src/lib.rs index 6542a7a1..b6600a54 100644 --- a/serial/src/lib.rs +++ b/serial/src/lib.rs @@ -4,7 +4,7 @@ use std::{ops::Deref, sync::Arc, time::Duration}; use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; -use unros_core::{ +use unros::{ anyhow, async_trait, bytes::Bytes, pubsub::{Publisher, Subscriber, Subscription}, diff --git a/serial/src/main.rs b/serial/src/main.rs index 81678c8d..da0ba797 100644 --- a/serial/src/main.rs +++ b/serial/src/main.rs @@ -1,53 +1,48 @@ use std::io::{stdin, stdout, BufRead, Write}; use serial::SerialConnection; -use unros_core::{ +use unros::{ anyhow, bytes::Bytes, - default_run_options, pubsub::{Publisher, Subscriber}, - spawn_persistent_thread, start_unros_runtime, tokio, + spawn_persistent_thread, tokio, Application, }; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - |mut app| async { - // "/dev/serial/by-id/usb-MicroPython_Board_in_FS_mode_e6616407e3496e28-if00" - let mut serial = SerialConnection::new("/dev/ttyACM1".into(), 115200, true).await; - let mut sub = Subscriber::::new(8); - serial.accept_msg_received_sub(sub.create_subscription()); - tokio::spawn(async move { - loop { - let x = sub.recv().await; - let stdout = stdout(); - let mut stdout = stdout.lock(); - stdout - .write_all(&x) - .expect("Stdout should have been writable"); - stdout.flush().expect("Stdout should have been flushable"); - } - }); - let mut write_signal: Publisher<_> = Default::default(); - write_signal.accept_subscription(serial.create_message_to_send_sub()); +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + // "/dev/serial/by-id/usb-MicroPython_Board_in_FS_mode_e6616407e3496e28-if00" + let mut serial = SerialConnection::new("/dev/ttyACM1".into(), 115200, true).await; + let mut sub = Subscriber::::new(8); + serial.accept_msg_received_sub(sub.create_subscription()); + tokio::spawn(async move { + loop { + let x = sub.recv().await; + let stdout = stdout(); + let mut stdout = stdout.lock(); + stdout + .write_all(&x) + .expect("Stdout should have been writable"); + stdout.flush().expect("Stdout should have been flushable"); + } + }); + let mut write_signal: Publisher<_> = Default::default(); + write_signal.accept_subscription(serial.create_message_to_send_sub()); - spawn_persistent_thread(move || { - let stdin = stdin(); - let mut stdin = stdin.lock(); + spawn_persistent_thread(move || { + let stdin = stdin(); + let mut stdin = stdin.lock(); - let mut line = String::new(); - loop { - line.clear(); - stdin.read_line(&mut line).expect("Failed to read a line"); - line += "\r"; - let line = line.clone().into_bytes(); - write_signal.set(line.into()); - } - }); + let mut line = String::new(); + loop { + line.clear(); + stdin.read_line(&mut line).expect("Failed to read a line"); + line += "\r"; + let line = line.clone().into_bytes(); + write_signal.set(line.into()); + } + }); - app.add_node(serial); + app.add_node(serial); - Ok(app) - }, - default_run_options!(), - ) + Ok(app) } diff --git a/smooth-diff-drive/Cargo.toml b/smooth-diff-drive/Cargo.toml index 45e01498..2fabb509 100644 --- a/smooth-diff-drive/Cargo.toml +++ b/smooth-diff-drive/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } machine-academy = { git = "https://github.com/manglemix/machine-academy.git" } serde = { workspace = true } rand = { workspace = true } diff --git a/smooth-diff-drive/src/drive.rs b/smooth-diff-drive/src/drive.rs index f65732c0..d7693096 100644 --- a/smooth-diff-drive/src/drive.rs +++ b/smooth-diff-drive/src/drive.rs @@ -1,4 +1,7 @@ -use std::{cmp::Ordering, fmt::{Debug, Display}}; +use std::{ + cmp::Ordering, + fmt::{Debug, Display}, +}; use serde::{Deserialize, Serialize}; @@ -52,7 +55,7 @@ impl From for f32 { match value.drive.cmp(&0) { Ordering::Greater => value.drive as f32 / 127.0, Ordering::Less => value.drive as f32 / 128.0, - Ordering::Equal => 0.0 + Ordering::Equal => 0.0, } // if value.drive == 0 { // 0.0 diff --git a/telemetry/Cargo.toml b/telemetry/Cargo.toml index 429613ba..9884481b 100644 --- a/telemetry/Cargo.toml +++ b/telemetry/Cargo.toml @@ -6,7 +6,7 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -unros-core = { path = "../unros-core" } +unros = { path = "../unros" } enet = "0.3.0" global-msgs = { path = "../global-msgs" } num_enum = "0.7.1" diff --git a/telemetry/src/lib.rs b/telemetry/src/lib.rs index b61395cb..4ff6d7d7 100644 --- a/telemetry/src/lib.rs +++ b/telemetry/src/lib.rs @@ -12,7 +12,7 @@ use image::DynamicImage; use num_enum::{IntoPrimitive, TryFromPrimitive}; use ordered_float::NotNan; use spin_sleep::SpinSleeper; -use unros_core::{ +use unros::{ anyhow, async_trait, log, logging::dump::{ScalingFilter, VideoDataDump}, pubsub::{Publisher, Subscriber, Subscription}, diff --git a/telemetry/src/main.rs b/telemetry/src/main.rs index 7c81cd52..04ddd3d6 100644 --- a/telemetry/src/main.rs +++ b/telemetry/src/main.rs @@ -1,22 +1,18 @@ use std::{net::SocketAddrV4, str::FromStr}; use telemetry::Telemetry; -use unros_core::{anyhow, default_run_options, start_unros_runtime}; +use unros::{anyhow, Application}; -fn main() -> anyhow::Result<()> { - start_unros_runtime( - move |mut app| async { - let telemetry = Telemetry::new( - SocketAddrV4::from_str("10.8.0.6:43721").unwrap(), - 1280, - 720, - 18, - ) - .await?; - - app.add_node(telemetry); - Ok(app) - }, - default_run_options!(), +#[unros::main] +async fn main(mut app: Application) -> anyhow::Result { + let telemetry = Telemetry::new( + SocketAddrV4::from_str("10.8.0.6:43721").unwrap(), + 1280, + 720, + 18, ) + .await?; + + app.add_node(telemetry); + Ok(app) } diff --git a/unros-core/src/lib.rs b/unros-core/src/lib.rs index 2eb51dd9..78f6c62b 100644 --- a/unros-core/src/lib.rs +++ b/unros-core/src/lib.rs @@ -419,7 +419,7 @@ pub fn get_env<'de, T: Deserialize<'de>>() -> anyhow::Result { enum EndCondition { CtrlC, - Dropped + Dropped, } pub fn start_unros_runtime> + Send + 'static>( @@ -437,8 +437,7 @@ pub fn start_unros_runtime> + Sen continue; } let cpus = sys.cpus(); - let usage = - cpus.iter().map(|cpu| cpu.cpu_usage()).sum::() / cpus.len() as f32; + let usage = cpus.iter().map(|cpu| cpu.cpu_usage()).sum::() / cpus.len() as f32; if usage >= 80.0 { warn!("CPU Usage at {usage}%"); last_cpu_check = Instant::now(); diff --git a/unros-core/src/logging/dump.rs b/unros-core/src/logging/dump.rs index 5d76ee46..ba31675b 100644 --- a/unros-core/src/logging/dump.rs +++ b/unros-core/src/logging/dump.rs @@ -381,12 +381,14 @@ a=rtpmap:96 H265/90000", let dump_type2 = dump_type.clone(); spawn_persistent_thread(move || { - events.for_each(|event| if let FfmpegEvent::Log(level, msg) = event { - match level { - ffmpeg_sidecar::event::LogLevel::Info => info!("[{dump_type2}] {msg}"), - ffmpeg_sidecar::event::LogLevel::Warning => warn!("[{dump_type2}] {msg}"), - ffmpeg_sidecar::event::LogLevel::Unknown => {} - _ => error!("[{dump_type2}] {msg}"), + events.for_each(|event| { + if let FfmpegEvent::Log(level, msg) = event { + match level { + ffmpeg_sidecar::event::LogLevel::Info => info!("[{dump_type2}] {msg}"), + ffmpeg_sidecar::event::LogLevel::Warning => warn!("[{dump_type2}] {msg}"), + ffmpeg_sidecar::event::LogLevel::Unknown => {} + _ => error!("[{dump_type2}] {msg}"), + } } }); }); diff --git a/unros-core/src/logging/mod.rs b/unros-core/src/logging/mod.rs index 865ce329..c4068eaa 100644 --- a/unros-core/src/logging/mod.rs +++ b/unros-core/src/logging/mod.rs @@ -144,9 +144,7 @@ pub(super) fn init_logger(run_options: &RunOptions) -> anyhow::Result<()> { // This filter is to avoid logging panics to the console, since rust already does that. // Note that the 'panic' target is set by us in eyre.rs. .filter(|x| x.target() != "panic") - .filter(|x| { - !(x.target() == "unros_core::logging::dump" && x.level() == Level::Info) - }) + .filter(|x| !(x.target() == "unros::logging::dump" && x.level() == Level::Info)) .format(move |out, message, record| { let secs = START_TIME.get().unwrap().elapsed().as_secs_f32(); out.finish(format_args!( diff --git a/unros-core/src/service/mod.rs b/unros-core/src/service/mod.rs index 97d2cb25..8077295b 100644 --- a/unros-core/src/service/mod.rs +++ b/unros-core/src/service/mod.rs @@ -1,24 +1,42 @@ use tokio::sync::{mpsc, oneshot}; pub struct ServiceHandle { - schedule_sender: mpsc::UnboundedSender> + schedule_sender: mpsc::UnboundedSender< + ScheduleRequest, + >, } - -impl Clone for ServiceHandle { +impl Clone + for ServiceHandle +{ fn clone(&self) -> Self { - Self { schedule_sender: self.schedule_sender.clone() } + Self { + schedule_sender: self.schedule_sender.clone(), + } } } -impl ServiceHandle { - pub async fn try_schedule_or_closed(&self, input: ScheduleInput) -> Option, ScheduleError>> { +impl + ServiceHandle +{ + pub async fn try_schedule_or_closed( + &self, + input: ScheduleInput, + ) -> Option, ScheduleError>> { let (sender, receiver) = oneshot::channel(); - self.schedule_sender.send(ScheduleRequest { input: Some(input), sender }).ok()?; + self.schedule_sender + .send(ScheduleRequest { + input: Some(input), + sender, + }) + .ok()?; receiver.await.ok() } - pub async fn try_schedule(&self, input: ScheduleInput) -> Result, ScheduleError> { + pub async fn try_schedule( + &self, + input: ScheduleInput, + ) -> Result, ScheduleError> { let Some(out) = self.try_schedule_or_closed(input).await else { std::future::pending::<()>().await; unreachable!() @@ -27,50 +45,63 @@ impl ServiceHandle { data: Option, - output_recv: oneshot::Receiver + output_recv: oneshot::Receiver, } - pub struct ScheduleRequest { input: Option, - sender: oneshot::Sender, ScheduleError>> + sender: oneshot::Sender, ScheduleError>>, } - pub struct Pending { - output_sender: oneshot::Sender + output_sender: oneshot::Sender, } - pub struct Service { - schedule_recv: mpsc::UnboundedReceiver> + schedule_recv: mpsc::UnboundedReceiver< + ScheduleRequest, + >, } - -pub fn new_service() -> (Service, ServiceHandle) { +pub fn new_service() -> ( + Service, + ServiceHandle, +) { let (schedule_sender, schedule_recv) = mpsc::unbounded_channel(); (Service { schedule_recv }, ServiceHandle { schedule_sender }) } - -impl Service { - pub async fn wait_for_request(&mut self) -> Option> { +impl + Service +{ + pub async fn wait_for_request( + &mut self, + ) -> Option> { self.schedule_recv.recv().await } - pub fn blocking_wait_for_request(&mut self) -> Option> { + pub fn blocking_wait_for_request( + &mut self, + ) -> Option> { self.schedule_recv.blocking_recv() } } - -impl ScheduleRequest { +impl + ScheduleRequest +{ pub fn accept(self, data: ScheduleData) -> Option> { let (output_sender, output_recv) = oneshot::channel(); - if self.sender.send(Ok(ScheduledService { data: Some(data), output_recv })).is_err() { + if self + .sender + .send(Ok(ScheduledService { + data: Some(data), + output_recv, + })) + .is_err() + { None } else { Some(Pending { output_sender }) @@ -94,7 +125,6 @@ impl ScheduleRequest Pending { pub fn finish(self, output: TaskOutput) { let _ = self.output_sender.send(output); @@ -105,7 +135,6 @@ impl Pending { } } - impl ScheduledService { pub fn get_data(&self) -> Option<&ScheduleData> { self.data.as_ref() diff --git a/unros-macros/Cargo.toml b/unros-macros/Cargo.toml new file mode 100644 index 00000000..3809c5dc --- /dev/null +++ b/unros-macros/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "unros-macros" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[lib] +proc-macro = true + +[dependencies] +quote = "1.0" +syn = "2.0" diff --git a/unros-macros/src/lib.rs b/unros-macros/src/lib.rs new file mode 100644 index 00000000..2420e633 --- /dev/null +++ b/unros-macros/src/lib.rs @@ -0,0 +1,47 @@ +extern crate proc_macro; + +use proc_macro::TokenStream; +use quote::quote; +use syn::{parse_macro_input, ItemFn}; + +#[proc_macro_attribute] +pub fn main(attr: TokenStream, item: TokenStream) -> TokenStream { + if !attr.is_empty() { + return quote! { compile_error!("unros::main does not accept attributes"); }.into(); + } + let input: ItemFn = parse_macro_input!(item); + let ident = input.sig.ident.clone(); + + if input.sig.asyncness.is_none() { + return quote! { compile_error!("The function must be `async`"); }.into(); + } + + if input.sig.inputs.len() != 1 { + return quote! { compile_error!("The function must have exactly 1 parameter"); }.into(); + } + + // let param = input.sig.inputs.first_mut().unwrap(); + // let FnArg::Typed(ref mut param) = param else { panic!("Invalid parameter"); }; + // if let Type::Infer(_) = param.ty.deref() { + // param.ty = Box::new(Type::Verbatim(quote!{ unros::Application })); + // } + + // match &input.sig.output { + // ReturnType::Type(_, ty) => match ty.deref() { + // Type::Path(x) => panic!("{x:?}"), + // _ => panic!("The method must return an `Application` object"), + // } + // _ => panic!("The method must return an `Application` object"), + // } + + quote! { + fn main() -> unros::anyhow::Result<()> { + #input + unros::start_unros_runtime( + #ident, + unros::default_run_options!(), + ) + } + } + .into() +} diff --git a/unros/Cargo.toml b/unros/Cargo.toml new file mode 100644 index 00000000..28e2b71e --- /dev/null +++ b/unros/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "unros" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +unros-core = { path = "../unros-core" } +unros-macros = { path = "../unros-macros" } \ No newline at end of file diff --git a/unros/src/lib.rs b/unros/src/lib.rs new file mode 100644 index 00000000..b4e65856 --- /dev/null +++ b/unros/src/lib.rs @@ -0,0 +1,2 @@ +pub use unros_core::*; +pub use unros_macros::*;