From e6c31c876c76c7607d77b5f8a5dded305f934e19 Mon Sep 17 00:00:00 2001 From: AJ Williams Date: Fri, 25 Jul 2025 00:49:36 -0700 Subject: [PATCH 1/2] BROKEN: take log path as arg --- src/main.rs | 7 ++++++- src/telemetry/telemetry_main.rs | 15 +++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 87ea93c..3813d22 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,6 +60,10 @@ pub struct Args { /// derivative gain scale #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] kd_scale: f64, + + /// derivative gain scale + #[arg(long, value_name = "PATH", default_value = "events.log")] + kinfer_log_path: String, } async fn driver() -> std::io::Result<()> { @@ -95,11 +99,12 @@ async fn driver() -> std::io::Result<()> { } fn main() { + let args = Args::parse(); // Setup telemetry before we do anything else let (tx, rx) = mpsc::sync_channel::(1024 * 1024); // Spawn the thread that will format and log our data - let jh = start_pipeline(rx, "events.log").expect("Failed to start telemetry pipeline"); + let jh = start_pipeline(rx, &args.kinfer_log_path).expect("Failed to start telemetry pipeline"); let trace_only_filter = tracing_subscriber::filter::FilterFn::new(|metadata: &Metadata| { metadata.level() == &Level::TRACE && metadata.target().starts_with("faux_rtos") diff --git a/src/telemetry/telemetry_main.rs b/src/telemetry/telemetry_main.rs index ccad0f3..f85f9e5 100644 --- a/src/telemetry/telemetry_main.rs +++ b/src/telemetry/telemetry_main.rs @@ -5,6 +5,7 @@ use std::os::unix::io::{AsFd, AsRawFd, OwnedFd}; use std::net::UdpSocket; use std::os::unix::fs::OpenOptionsExt; +use std::path::PathBuf; use std::ptr; use std::thread; use std::time::Instant; @@ -24,6 +25,20 @@ pub fn start_pipeline( rx: Receiver, log_path: &str, ) -> io::Result> { + + println!("log path: {:?}", log_path); + + let log_path = std::path::Path::new(log_path); + + if let Some(parent) = log_path.parent() { + std::fs::create_dir_all(parent)?; + } + + let log_path = log_path.canonicalize() + .unwrap_or_else(|_| panic!("Failed to canonicalize log path")); + + println!("log path: {:?}", log_path); + // Open log file for writing let file = OpenOptions::new() .create(true) From 1ecf1afd0bc9d553f82304530d5f3dffe344e17f Mon Sep 17 00:00:00 2001 From: AJ Williams Date: Fri, 25 Jul 2025 02:08:54 -0700 Subject: [PATCH 2/2] Add Config; pass log file as argument --- src/behavior.rs | 22 ++++++--------------- src/config.rs | 9 +++++++++ src/main.rs | 28 +++++++++++++++++++------- src/robot_description.rs | 35 ++++++--------------------------- src/telemetry/telemetry_main.rs | 15 ++++---------- 5 files changed, 46 insertions(+), 63 deletions(-) create mode 100644 src/config.rs diff --git a/src/behavior.rs b/src/behavior.rs index 5b97f7b..f4639ba 100644 --- a/src/behavior.rs +++ b/src/behavior.rs @@ -9,6 +9,8 @@ use std::{ }; use tracing::{debug, error, info, warn}; +use crate::config::Config; + use crate::imu::{self, ImuManager}; use crate::robot_description::{self, ActuatorId, RobotDescription}; @@ -38,15 +40,9 @@ pub struct Store { kb_manager: crate::keyboard::KeyboardManager, } -impl Default for Store { - fn default() -> Self { - Self::new() - } -} - impl Store { - pub fn new() -> Self { - let robot_description = RobotDescription::new(); + pub fn new(config: &Config) -> Self { + let robot_description = RobotDescription::new(config); let model_manager = ModelManager::new("model.kinfer", &robot_description) .expect("Failed to create model manager"); @@ -617,17 +613,11 @@ pub struct BehaviorManager { pending_fut: Option, } -impl Default for BehaviorManager { - fn default() -> Self { - Self::new() - } -} - impl BehaviorManager { - pub fn new() -> Self { + pub fn new(config: &Config) -> Self { Self { state: Some(StateStore::Reset(Reset { - shared_state: Box::pin(Store::new()), + shared_state: Box::pin(Store::new(config)), })), target: None, pending_fut: None, diff --git a/src/config.rs b/src/config.rs new file mode 100644 index 0000000..a684b4e --- /dev/null +++ b/src/config.rs @@ -0,0 +1,9 @@ +use std::path::PathBuf; + +#[derive(Debug)] +pub struct Config { + pub policy_scale: f64, + pub kp_scale: f64, + pub kd_scale: f64, + pub log_path: PathBuf, +} diff --git a/src/main.rs b/src/main.rs index 3813d22..5a727fd 100644 --- a/src/main.rs +++ b/src/main.rs @@ -8,6 +8,7 @@ pub mod actuator; pub mod actuator_manager; pub mod behavior; pub mod bytestream_fd; +pub mod config; pub mod imu; pub mod inference; pub mod keyboard; @@ -28,6 +29,8 @@ use std::task::{Context, Poll}; use crate::robstride::{ObtainIdRequest, ObtainIdResponse}; +use config::Config; + use socketcan::CanFrame; pub mod robstride; pub mod robstride_utils; @@ -44,10 +47,11 @@ use tracing::{Level, Metadata, debug, error, info, trace, warn}; use tracing_subscriber::{EnvFilter, Layer, fmt, layer::SubscriberExt}; use std::sync::mpsc; +use std::path::PathBuf; use clap::Parser; #[derive(Debug, Parser)] -#[command(name = "faux-rtos", about = "Parse three floats")] +#[command(name = "faux-rtos", about = "Parse three floats and a path")] pub struct Args { /// scale factor for the policy #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] @@ -61,13 +65,13 @@ pub struct Args { #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] kd_scale: f64, - /// derivative gain scale + /// path to log file #[arg(long, value_name = "PATH", default_value = "events.log")] - kinfer_log_path: String, + kinfer_log_path: PathBuf, } -async fn driver() -> std::io::Result<()> { - let mut behavior_manager = behavior::BehaviorManager::new(); +async fn driver(config: &Config) -> std::io::Result<()> { + let mut behavior_manager = behavior::BehaviorManager::new(config); let mut pinned = unsafe { Pin::new_unchecked(&mut behavior_manager) }; loop { // iterate over each SlowCounter in sc_vec @@ -99,12 +103,22 @@ async fn driver() -> std::io::Result<()> { } fn main() { + // Parse command line arguments let args = Args::parse(); + + let config = Config { + policy_scale: args.policy_scale, + kp_scale: args.kp_scale, + kd_scale: args.kd_scale, + log_path: args.kinfer_log_path, + }; + + // Setup telemetry before we do anything else let (tx, rx) = mpsc::sync_channel::(1024 * 1024); // Spawn the thread that will format and log our data - let jh = start_pipeline(rx, &args.kinfer_log_path).expect("Failed to start telemetry pipeline"); + let jh = start_pipeline(rx, &config.log_path).expect("Failed to start telemetry pipeline"); let trace_only_filter = tracing_subscriber::filter::FilterFn::new(|metadata: &Metadata| { metadata.level() == &Level::TRACE && metadata.target().starts_with("faux_rtos") @@ -142,7 +156,7 @@ fn main() { // handle driver and SIGINT let drv = async { tokio::select! { - result = driver() => { + result = driver(&config) => { match result { Ok(_) => { info!("Driver finished successfully"); diff --git a/src/robot_description.rs b/src/robot_description.rs index b6982a4..465aab9 100644 --- a/src/robot_description.rs +++ b/src/robot_description.rs @@ -4,6 +4,8 @@ use heapless::Deque; use nalgebra as na; use tracing::info; +use crate::config::Config; + pub fn normalize_actuator_qpos(mut qpos: f64) -> f64 { const TWO_PI: f64 = 2.0 * std::f64::consts::PI; // rem_euclid gives a value in [0, 2π) @@ -310,23 +312,6 @@ pub enum DataType { Time, } -use clap::Parser; -#[derive(Debug, Parser)] -#[command(name = "faux-rtos", about = "Parse three floats")] -pub struct Args { - /// scale factor for the policy - #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] - policy_scale: f64, - - /// proportional gain scale - #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] - kp_scale: f64, - - /// derivative gain scale - #[arg(long, value_name = "FLOAT", default_value_t = 1.0)] - kd_scale: f64, -} - pub struct RobotDescription { pub actuators: ActuatorStateStore, pub imu: ImuData, @@ -339,24 +324,16 @@ pub struct RobotDescription { pub kd_scale: f64, } -impl Default for RobotDescription { - fn default() -> Self { - Self::new() - } -} - impl RobotDescription { - pub fn new() -> Self { - let args = Args::parse(); - info!("Args; {:?}", args); + pub fn new(config: &Config) -> Self { Self { actuators: ActuatorStateStore::new(), imu: ImuData::default(), initial_imu: ImuData::default(), kb_pending_events: Deque::new(), - kp_scale: args.kp_scale, - kd_scale: args.kd_scale, - policy_scale: args.policy_scale, + kp_scale: config.kp_scale, + kd_scale: config.kd_scale, + policy_scale: config.policy_scale, home_position: enum_map! { ActuatorId::Lsp => ActuatorCommand { qpos: 0.0, kp: 100.0, kd: 8.284, ..Default::default() }, ActuatorId::Lsr => ActuatorCommand { qpos: (10.0_f64).to_radians(), kp: 100.0, kd: 8.257, ..Default::default() }, diff --git a/src/telemetry/telemetry_main.rs b/src/telemetry/telemetry_main.rs index f85f9e5..7d06344 100644 --- a/src/telemetry/telemetry_main.rs +++ b/src/telemetry/telemetry_main.rs @@ -5,14 +5,14 @@ use std::os::unix::io::{AsFd, AsRawFd, OwnedFd}; use std::net::UdpSocket; use std::os::unix::fs::OpenOptionsExt; -use std::path::PathBuf; +use std::path::Path; use std::ptr; use std::thread; use std::time::Instant; use nix::libc; use std::sync::mpsc::Receiver; -use tracing::error; +use tracing::{info, error}; use crate::telemetry::forwarder::{EventRecord, FieldValue}; use crate::telemetry::multi_fd_writer::MultiFdWriter; @@ -23,21 +23,14 @@ const BUF_SIZE: usize = 4 * 4096; /// spawns a thread that will handle the I/O operations (read events and write to disk) pub fn start_pipeline( rx: Receiver, - log_path: &str, + log_path: &Path, ) -> io::Result> { - println!("log path: {:?}", log_path); - - let log_path = std::path::Path::new(log_path); - if let Some(parent) = log_path.parent() { std::fs::create_dir_all(parent)?; } - let log_path = log_path.canonicalize() - .unwrap_or_else(|_| panic!("Failed to canonicalize log path")); - - println!("log path: {:?}", log_path); + info!("Logging to: {:?}", log_path); // Open log file for writing let file = OpenOptions::new()