From 7658a01a618878ed1b093728184cfff4a5545171 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 12:45:45 -0800 Subject: [PATCH 01/37] rename packages --- pykos/Makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pykos/Makefile b/pykos/Makefile index f47ff64..d5c35f9 100644 --- a/pykos/Makefile +++ b/pykos/Makefile @@ -38,6 +38,10 @@ generate-proto: mv kos/* kos_protos/ rm -rf kos touch kos_protos/__init__.py + + # Fix imports in all generated files + find kos_protos -type f -name "*.py" -exec sed -i '' 's/from kos/from kos_protos/g' {} + + find kos_protos -type f -name "*.py" -exec sed -i '' 's/import kos/import kos_protos/g' {} + .PHONY: generate-proto From 2c48c6bb37f23b1dd442c6906c134d21e8e8f2f1 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 12:46:42 -0800 Subject: [PATCH 02/37] bump version to 0.1.3 --- pykos/pykos/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 47a9c1d..174e74a 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,3 +1,3 @@ -__version__ = "0.1.2" +__version__ = "0.1.3" from pykos.client import KOS From 86a5aacc53693b8048bc974d9413a2b5119ce30f Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 12:48:15 -0800 Subject: [PATCH 03/37] make work for linux --- pykos/Makefile | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/pykos/Makefile b/pykos/Makefile index d5c35f9..de2fe3a 100644 --- a/pykos/Makefile +++ b/pykos/Makefile @@ -40,8 +40,16 @@ generate-proto: touch kos_protos/__init__.py # Fix imports in all generated files - find kos_protos -type f -name "*.py" -exec sed -i '' 's/from kos/from kos_protos/g' {} + - find kos_protos -type f -name "*.py" -exec sed -i '' 's/import kos/import kos_protos/g' {} + + case "$$(uname)" in \ + Darwin) \ + find kos_protos -type f -name "*.py" -exec sed -i '' 's/from kos/from kos_protos/g' {} + && \ + find kos_protos -type f -name "*.py" -exec sed -i '' 's/import kos/import kos_protos/g' {} + \ + ;; \ + *) \ + find kos_protos -type f -name "*.py" -exec sed -i 's/from kos/from kos_protos/g' {} + && \ + find kos_protos -type f -name "*.py" -exec sed -i 's/import kos/import kos_protos/g' {} + \ + ;; \ + esac .PHONY: generate-proto From 907b5d0f18d1924183df17f5e0ee07c50be49eea Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 12:58:18 -0800 Subject: [PATCH 04/37] more fixes --- pykos/pykos/__init__.py | 1 + pykos/pykos/services/actuator.py | 4 ++-- pykos/pykos/services/imu.py | 2 +- pykos/pykos/services/process_manager.py | 6 +++--- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 174e74a..374c828 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,3 +1,4 @@ __version__ = "0.1.3" from pykos.client import KOS +from pykos.services import * diff --git a/pykos/pykos/services/actuator.py b/pykos/pykos/services/actuator.py index 5c836fc..e49f715 100644 --- a/pykos/pykos/services/actuator.py +++ b/pykos/pykos/services/actuator.py @@ -5,8 +5,8 @@ import grpc from google.longrunning import operations_pb2, operations_pb2_grpc from google.protobuf.any_pb2 import Any as AnyPb2 -from kos import actuator_pb2, actuator_pb2_grpc, common_pb2 -from kos.actuator_pb2 import CalibrateActuatorMetadata +from kos_protos import actuator_pb2, actuator_pb2_grpc, common_pb2 +from kos_protos.actuator_pb2 import CalibrateActuatorMetadata class CalibrationStatus: diff --git a/pykos/pykos/services/imu.py b/pykos/pykos/services/imu.py index acccd28..832f27d 100644 --- a/pykos/pykos/services/imu.py +++ b/pykos/pykos/services/imu.py @@ -2,7 +2,7 @@ import grpc from google.protobuf.empty_pb2 import Empty -from kos import imu_pb2, imu_pb2_grpc +from kos_protos import imu_pb2, imu_pb2_grpc class ImuValues: diff --git a/pykos/pykos/services/process_manager.py b/pykos/pykos/services/process_manager.py index 91710c8..a700fcb 100644 --- a/pykos/pykos/services/process_manager.py +++ b/pykos/pykos/services/process_manager.py @@ -4,9 +4,9 @@ import grpc from google.protobuf.empty_pb2 import Empty -from kos import process_manager_pb2_grpc -from kos.common_pb2 import Error -from kos.process_manager_pb2 import KClipStartRequest +from kos_protos import process_manager_pb2_grpc +from kos_protos.common_pb2 import Error +from kos_protos.process_manager_pb2 import KClipStartRequest class ProcessManagerServiceClient: From b2ff2d87ba9648fc12cc43cc07fef91a7983b1fe Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 12:58:37 -0800 Subject: [PATCH 05/37] bump version to 0.1.4 --- pykos/pykos/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 374c828..5e494b6 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,4 +1,4 @@ -__version__ = "0.1.3" +__version__ = "0.1.4" from pykos.client import KOS from pykos.services import * From 30ed68693bd689c5990b68a7b9c1baf1e7b2d9ff Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 13:06:03 -0800 Subject: [PATCH 06/37] lint --- pykos/pykos/services/actuator.py | 1 + pykos/pykos/services/imu.py | 1 + pykos/pykos/services/process_manager.py | 1 + pykos/pyproject.toml | 11 +++++++---- 4 files changed, 10 insertions(+), 4 deletions(-) diff --git a/pykos/pykos/services/actuator.py b/pykos/pykos/services/actuator.py index e49f715..ed4be3a 100644 --- a/pykos/pykos/services/actuator.py +++ b/pykos/pykos/services/actuator.py @@ -5,6 +5,7 @@ import grpc from google.longrunning import operations_pb2, operations_pb2_grpc from google.protobuf.any_pb2 import Any as AnyPb2 + from kos_protos import actuator_pb2, actuator_pb2_grpc, common_pb2 from kos_protos.actuator_pb2 import CalibrateActuatorMetadata diff --git a/pykos/pykos/services/imu.py b/pykos/pykos/services/imu.py index 832f27d..1af283d 100644 --- a/pykos/pykos/services/imu.py +++ b/pykos/pykos/services/imu.py @@ -2,6 +2,7 @@ import grpc from google.protobuf.empty_pb2 import Empty + from kos_protos import imu_pb2, imu_pb2_grpc diff --git a/pykos/pykos/services/process_manager.py b/pykos/pykos/services/process_manager.py index a700fcb..d83f9b4 100644 --- a/pykos/pykos/services/process_manager.py +++ b/pykos/pykos/services/process_manager.py @@ -4,6 +4,7 @@ import grpc from google.protobuf.empty_pb2 import Empty + from kos_protos import process_manager_pb2_grpc from kos_protos.common_pb2 import Error from kos_protos.process_manager_pb2 import KClipStartRequest diff --git a/pykos/pyproject.toml b/pykos/pyproject.toml index 4c8a3ef..a6bf9f4 100644 --- a/pykos/pyproject.toml +++ b/pykos/pyproject.toml @@ -42,10 +42,13 @@ module = [ "google.longrunning.*", "kos.*", "pykos.*", - "kos.actuator_pb2_grpc", - "kos.imu_pb2_grpc", - "kos.actuator_pb2", - "kos.imu_pb2" + "kos_protos.actuator_pb2_grpc", + "kos_protos.process_manager_pb2_grpc", + "kos_protos.imu_pb2_grpc", + "kos_protos.common_pb2", + "kos_protos.actuator_pb2", + "kos_protos.process_manager_pb2", + "kos_protos.imu_pb2" ] ignore_missing_imports = true follow_imports = "skip" From ca8082eaf78d16fbc26f5f060c19e7edaeea7a93 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 13:57:01 -0800 Subject: [PATCH 07/37] fix deserialize --- kos_core/src/services/krec_logger.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/kos_core/src/services/krec_logger.rs b/kos_core/src/services/krec_logger.rs index db18979..0956cd0 100644 --- a/kos_core/src/services/krec_logger.rs +++ b/kos_core/src/services/krec_logger.rs @@ -176,6 +176,9 @@ impl TelemetryLogger { match serde_json::from_slice::(&payload) { Ok(command_data) => { frame.inference_step = command_data.inference_step; + frame.video_timestamp = command_data.video_timestamp; + frame.frame_number = command_data.frame_number; + for item in command_data.data { frame.actuator_commands.push(ActuatorCommand { actuator_id: item.actuator_id, @@ -197,6 +200,7 @@ impl TelemetryLogger { if frame.inference_step > *current { // Add frame to KRec let mut krec = krec_clone.lock().await; + krec.add_frame(frame.clone()); // Save every 500 frames From 5831ca6fd9c3d177de95c008848b8d8ef78fdbb1 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 14:14:00 -0800 Subject: [PATCH 08/37] fix everything forever --- pykos/pykos/__init__.py | 3 ++- pykos/pykos/services/__init__.py | 0 pykos/setup.py | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) create mode 100644 pykos/pykos/services/__init__.py diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 5e494b6..0b48491 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,4 +1,5 @@ __version__ = "0.1.4" from pykos.client import KOS -from pykos.services import * + +from . import services diff --git a/pykos/pykos/services/__init__.py b/pykos/pykos/services/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pykos/setup.py b/pykos/setup.py index fa69d74..e25caba 100644 --- a/pykos/setup.py +++ b/pykos/setup.py @@ -68,7 +68,7 @@ def run(self) -> None: python_requires=">=3.8", install_requires=requirements, extras_require={"dev": requirements_dev}, - packages=["pykos", "kos_protos"], + packages=["pykos", "pykos.services", "kos_protos"], package_data={ "pykos": ["py.typed"], "kos_protos": ["py.typed"], From 6ed70dd4a45aabfbfe1d8dfe7e1de26fbd9267ef Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Mon, 25 Nov 2024 14:15:02 -0800 Subject: [PATCH 09/37] bump version to 0.1.5 --- pykos/pykos/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 0b48491..44209bd 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,4 +1,4 @@ -__version__ = "0.1.4" +__version__ = "0.1.5" from pykos.client import KOS From 9cb9a454edb868a991b14c6ece3e3aa4ef6ef9e4 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Mon, 25 Nov 2024 15:45:36 -0800 Subject: [PATCH 10/37] lint updates --- pykos/pyproject.toml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/pykos/pyproject.toml b/pykos/pyproject.toml index a6bf9f4..4a79cbf 100644 --- a/pykos/pyproject.toml +++ b/pykos/pyproject.toml @@ -65,10 +65,9 @@ target-version = "py310" [tool.ruff.lint] -select = ["ANN", "D", "E", "F", "I", "N", "PGH", "PLC", "PLE", "PLR", "PLW", "W"] +select = ["ANN", "D", "E", "F", "G", "I", "N", "PGH", "PLC", "PLE", "PLR", "PLW", "TID", "W"] ignore = [ - "ANN101", "ANN102", "D101", "D102", "D103", "D104", "D105", "D106", "D107", "N812", "N817", "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PLR2004", @@ -83,7 +82,7 @@ dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" [tool.ruff.lint.isort] -known-first-party = ["pykos", "tests"] +known-first-party = ["pykos", "kos_protos", "tests"] combine-as-imports = true [tool.ruff.lint.pydocstyle] From a9159397c51e232ccb51605bf4b5607e5fdd6380 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 3 Dec 2024 06:24:42 +0000 Subject: [PATCH 11/37] robstridev2 --- daemon/src/main.rs | 6 +- kos_core/proto/kos/actuator.proto | 1 + kos_core/src/lib.rs | 13 +- platforms/kbot/Cargo.toml | 4 +- platforms/kbot/src/actuator.rs | 212 ++++++++++++++---------------- platforms/kbot/src/lib.rs | 100 ++++++-------- platforms/stub/src/lib.rs | 27 ++-- 7 files changed, 161 insertions(+), 202 deletions(-) diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 34fd48e..0513a30 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -46,7 +46,7 @@ async fn run_server( let addr = "0.0.0.0:50051".parse()?; let mut server_builder = Server::builder(); - let services = platform.create_services(operations_service.clone())?; + let services = platform.create_services(operations_service.clone()).await?; let operations_service = OperationsServer::new(operations_service); @@ -72,7 +72,9 @@ async fn main() -> Result<()> { .add_directive("h2=error".parse().unwrap()) .add_directive("grpc=error".parse().unwrap()) .add_directive("rumqttc=error".parse().unwrap()) - .add_directive("kos_core::telemetry=error".parse().unwrap()), + .add_directive("kos_core::telemetry=error".parse().unwrap()) + .add_directive("polling=error".parse().unwrap()) + .add_directive("async_io=error".parse().unwrap()), ) .init(); diff --git a/kos_core/proto/kos/actuator.proto b/kos_core/proto/kos/actuator.proto index 4b55763..e56cea0 100644 --- a/kos_core/proto/kos/actuator.proto +++ b/kos_core/proto/kos/actuator.proto @@ -59,6 +59,7 @@ message ConfigureActuatorRequest { optional float protection_time = 7; // Protection time in seconds optional bool torque_enabled = 8; // Torque enabled flag optional uint32 new_actuator_id = 9; // New actuator ID + optional bool zero_position = 10; // Instant zero position } // Request message for CalibrateActuator. diff --git a/kos_core/src/lib.rs b/kos_core/src/lib.rs index 275b4b9..a5ac1ee 100644 --- a/kos_core/src/lib.rs +++ b/kos_core/src/lib.rs @@ -11,12 +11,16 @@ pub mod telemetry_types; pub use grpc_interface::google as google_proto; pub use grpc_interface::kos as kos_proto; +use async_trait::async_trait; +use eyre::Result; use hal::actuator_service_server::ActuatorServiceServer; use hal::imu_service_server::ImuServiceServer; use hal::process_manager_service_server::ProcessManagerServiceServer; use services::OperationsServiceImpl; use services::{ActuatorServiceImpl, IMUServiceImpl, ProcessManagerServiceImpl}; use std::fmt::Debug; +use std::future::Future; +use std::pin::Pin; use std::sync::Arc; impl Debug for ActuatorServiceImpl { @@ -42,14 +46,15 @@ pub enum ServiceEnum { ProcessManager(ProcessManagerServiceServer), } -pub trait Platform { +#[async_trait] +pub trait Platform: Send + Sync { fn name(&self) -> &'static str; fn serial(&self) -> String; fn initialize(&mut self, operations_service: Arc) -> eyre::Result<()>; - fn create_services( - &self, + fn create_services<'a>( + &'a self, operations_service: Arc, - ) -> eyre::Result>; + ) -> Pin>> + Send + 'a>>; fn shutdown(&mut self) -> eyre::Result<()>; } diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index da9b71d..b613239 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -10,10 +10,10 @@ description = "KOS platform for KBot" [dependencies] kos_core = { version = "0.1.1", path = "../../kos_core" } eyre = "0.6" -krec = "0.1" +krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstride = "0.2.9" +robstridev2 = "0.1" gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 7ed66a2..22066f0 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -1,6 +1,6 @@ use crate::{Arc, Operation, OperationsServiceImpl}; use async_trait::async_trait; -use eyre::{Result, WrapErr}; +use eyre::Result; use kos_core::{ hal::{ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorRequest}, kos_proto::{ @@ -9,42 +9,53 @@ use kos_core::{ common::{ActionResult, ErrorCode}, }, }; -use std::collections::HashMap; -use robstride::{MotorType, MotorsSupervisor}; +use robstridev2::{CH341Transport, ControlConfig, SocketCanTransport, Supervisor, TransportType}; +use std::time::Duration; +use tokio::sync::Mutex; pub struct KBotActuator { - motors_supervisor: MotorsSupervisor, + supervisor: Arc>, } impl KBotActuator { - pub fn new( + pub async fn new( _operations_service: Arc, - port: &str, - motor_infos: HashMap, - verbose: Option, - max_update_rate: Option, - zero_on_init: Option, + ports: Vec<&str>, + motors_timeout: Duration, ) -> Result { - let motor_infos_u8 = motor_infos - .into_iter() - .map(|(k, v)| { - let id = - u8::try_from(k).wrap_err_with(|| format!("Motor ID {} too large for u8", k))?; - Ok((id, v)) - }) - .collect::>>()?; - - let motors_supervisor = MotorsSupervisor::new( - port, - &motor_infos_u8, - verbose.unwrap_or(false), - max_update_rate.unwrap_or(100000) as f64, - zero_on_init.unwrap_or(false), - ) - .map_err(|e| eyre::eyre!("Failed to create motors supervisor: {}", e))?; - - Ok(KBotActuator { motors_supervisor }) + let mut supervisor = Supervisor::new(motors_timeout)?; + + for port in ports.clone() { + if port.starts_with("/dev/tty") { + let serial = CH341Transport::new(port.to_string()).await?; + supervisor + .add_transport(format!("{}", port), TransportType::CH341(serial)) + .await?; + } else if port.starts_with("can") { + let can = SocketCanTransport::new(port.to_string()).await?; + supervisor + .add_transport(format!("{}", port), TransportType::SocketCAN(can)) + .await?; + } else { + return Err(eyre::eyre!("Invalid port: {}", port)); + } + } + + let mut supervisor_runner = supervisor.clone_controller(); + let _supervisor_handle = tokio::spawn(async move { + if let Err(e) = supervisor_runner.run().await { + tracing::error!("Supervisor task failed: {}", e); + } + }); + + for port in ports.clone() { + supervisor.scan_bus(0xFD, port).await?; + } + + Ok(KBotActuator { + supervisor: Arc::new(Mutex::new(supervisor)), + }) } } @@ -53,47 +64,25 @@ impl Actuator for KBotActuator { async fn command_actuators(&self, commands: Vec) -> Result> { let mut results = vec![]; for command in commands { - let mut motor_result = vec![]; - if let Some(position) = command.position { - let result = self - .motors_supervisor - .set_position(command.actuator_id as u8, position.to_radians() as f32); - motor_result.push(result); - } - if let Some(velocity) = command.velocity { - let result = self - .motors_supervisor - .set_velocity(command.actuator_id as u8, velocity as f32); - motor_result.push(result); - } - if let Some(torque) = command.torque { - let result = self - .motors_supervisor - .set_torque(command.actuator_id as u8, torque as f32); - motor_result.push(result); - } + let motor_id = command.actuator_id as u8; + let mut supervisor = self.supervisor.lock().await; + let result = supervisor + .command( + motor_id, + command + .position + .map(|p| p.to_radians() as f32) + .unwrap_or(0.0), + command.velocity.map(|v| v as f32).unwrap_or(0.0), + command.torque.map(|t| t as f32).unwrap_or(0.0), + ) + .await; - let success = motor_result.iter().all(|r| r.is_ok()); - let error = if !success { - Some(KosError { - code: if motor_result - .iter() - .any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound)) - { - ErrorCode::InvalidArgument as i32 - } else { - ErrorCode::HardwareFailure as i32 - }, - message: motor_result - .iter() - .filter_map(|r| r.as_ref().err()) - .map(|e| e.to_string()) - .collect::>() - .join("; "), - }) - } else { - None - }; + let success = result.is_ok(); + let error = result.err().map(|e| KosError { + code: ErrorCode::HardwareFailure as i32, + message: e.to_string(), + }); results.push(ActionResult { actuator_id: command.actuator_id, @@ -106,41 +95,31 @@ impl Actuator for KBotActuator { async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result { let motor_id = config.actuator_id as u8; - let mut results = vec![]; - // Configure KP if provided - if let Some(kp) = config.kp { - let result = self.motors_supervisor.set_kp(motor_id, kp as f32); - results.push(result); - } + let control_config = ControlConfig { + kp: config.kp.unwrap_or(0.0) as f32, + kd: config.kd.unwrap_or(0.0) as f32, + max_torque: Some(2.0), + max_velocity: Some(5.0), + max_current: Some(10.0), + }; + + let mut supervisor = self.supervisor.lock().await; + let result = supervisor.configure(motor_id, control_config).await; - // Configure KD if provided - if let Some(kd) = config.kd { - let result = self.motors_supervisor.set_kd(motor_id, kd as f32); - results.push(result); + if let Some(torque_enabled) = config.torque_enabled { + if torque_enabled { + supervisor.enable(motor_id).await?; + } else { + supervisor.disable(motor_id, true).await?; + } } - let success = results.iter().all(|r| r.is_ok()); - let error = if !success { - Some(kos_core::kos_proto::common::Error { - code: if results - .iter() - .any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound)) - { - ErrorCode::InvalidArgument as i32 - } else { - ErrorCode::HardwareFailure as i32 - }, - message: results - .iter() - .filter_map(|r| r.as_ref().err()) - .map(|e| e.to_string()) - .collect::>() - .join("; "), - }) - } else { - None - }; + let success = result.is_ok(); + let error = result.err().map(|e| KosError { + code: ErrorCode::HardwareFailure as i32, + message: e.to_string(), + }); Ok(ActionResponse { success, error }) } @@ -151,21 +130,24 @@ impl Actuator for KBotActuator { async fn get_actuators_state( &self, - _actuator_ids: Vec, + actuator_ids: Vec, ) -> Result> { - let feedback = self.motors_supervisor.get_latest_feedback(); - Ok(feedback - .iter() - .map(|(id, state)| ActuatorStateResponse { - actuator_id: u32::from(*id), - online: matches!(state.mode, robstride::MotorMode::Motor), - position: Some(state.position.to_degrees() as f64), - velocity: Some(state.velocity as f64), - torque: Some(state.torque as f64), - temperature: None, - voltage: None, - current: None, - }) - .collect()) + let mut responses = vec![]; + for id in actuator_ids { + let supervisor = self.supervisor.lock().await; + if let Ok(Some((feedback, ts))) = supervisor.get_feedback(id as u8).await { + responses.push(ActuatorStateResponse { + actuator_id: id, + online: ts.elapsed().unwrap_or(Duration::from_secs(1)) < Duration::from_secs(1), + position: Some(feedback.angle as f64), + velocity: Some(feedback.velocity as f64), + torque: Some(feedback.torque as f64), + temperature: Some(feedback.temperature as f64), + voltage: None, + current: None, + }); + } + } + Ok(responses) } } diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index e5a07ea..6c8a91b 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -10,6 +10,7 @@ pub use actuator::*; pub use hexmove::*; pub use process_manager::*; +use async_trait::async_trait; use eyre::{Result, WrapErr}; use kos_core::hal::Operation; use kos_core::kos_proto::actuator::actuator_service_server::ActuatorServiceServer; @@ -18,9 +19,11 @@ use kos_core::{ services::{ActuatorServiceImpl, OperationsServiceImpl, ProcessManagerServiceImpl}, Platform, ServiceEnum, }; -use robstride::MotorType; use std::collections::HashMap; +use std::future::Future; +use std::pin::Pin; use std::sync::Arc; +use std::time::Duration; pub struct KbotPlatform {} @@ -36,6 +39,7 @@ impl Default for KbotPlatform { } } +#[async_trait] impl Platform for KbotPlatform { fn name(&self) -> &'static str { "KBot" @@ -51,70 +55,40 @@ impl Platform for KbotPlatform { Ok(()) } - fn create_services( - &self, + fn create_services<'a>( + &'a self, operations_service: Arc, - ) -> Result> { - if cfg!(target_os = "linux") { - // Create the process manager first and handle any errors - let process_manager = KBotProcessManager::new(self.name().to_string(), self.serial()) - .wrap_err("Failed to initialize GStreamer process manager")?; + ) -> Pin>> + Send + 'a>> { + Box::pin(async move { + if cfg!(target_os = "linux") { + let process_manager = + KBotProcessManager::new(self.name().to_string(), self.serial()) + .wrap_err("Failed to initialize GStreamer process manager")?; - Ok(vec![ - // ServiceEnum::Imu( - // kos_core::kos_proto::imu::imu_service_server::ImuServiceServer::new( - // kos_core::services::IMUServiceImpl::new(Arc::new( - // KBotIMU::new(operations_service.clone(), "can0", 1, 1) - // .wrap_err("Failed to create IMU")?, - // )), - // ), - // ), - // TODO: fix config definition - ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( - Arc::new( - KBotActuator::new( - operations_service, - "/dev/ttyCH341USB1", - HashMap::from([ - (1, MotorType::Type03), - (2, MotorType::Type03), - (3, MotorType::Type01), - (4, MotorType::Type01), - (5, MotorType::Type01), - ]), - None, - None, - None, - ) - .wrap_err("Failed to create actuator")?, - ), - ))), - ServiceEnum::ProcessManager(ProcessManagerServiceServer::new( - ProcessManagerServiceImpl::new(Arc::new(process_manager)), - )), - ]) - } else { - Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( - ActuatorServiceImpl::new(Arc::new( - KBotActuator::new( - operations_service, - "/dev/ttyCH341USB0", - HashMap::from([ - (1, MotorType::Type04), - (2, MotorType::Type04), - (3, MotorType::Type04), - (4, MotorType::Type04), - (5, MotorType::Type04), - (6, MotorType::Type01), - ]), - None, - None, - None, - ) - .wrap_err("Failed to create actuator")?, - )), - ))]) - } + let actuator = + KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) + .await + .wrap_err("Failed to create actuator")?; + + Ok(vec![ + ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( + Arc::new(actuator), + ))), + ServiceEnum::ProcessManager(ProcessManagerServiceServer::new( + ProcessManagerServiceImpl::new(Arc::new(process_manager)), + )), + ]) + } else { + let actuator = + KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) + .await + .wrap_err("Failed to create actuator")?; + + Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( + ActuatorServiceImpl::new(Arc::new(actuator)), + ))]) + } + }) } fn shutdown(&mut self) -> eyre::Result<()> { diff --git a/platforms/stub/src/lib.rs b/platforms/stub/src/lib.rs index 321e89a..3220fac 100644 --- a/platforms/stub/src/lib.rs +++ b/platforms/stub/src/lib.rs @@ -5,6 +5,7 @@ pub use actuator::*; pub use imu::*; pub use process_manager::*; +use async_trait::async_trait; use eyre::Result; use kos_core::hal::Operation; use kos_core::kos_proto::{ @@ -14,6 +15,8 @@ use kos_core::kos_proto::{ }; use kos_core::services::{ActuatorServiceImpl, IMUServiceImpl, ProcessManagerServiceImpl}; use kos_core::{services::OperationsServiceImpl, Platform, ServiceEnum}; +use std::future::Future; +use std::pin::Pin; use std::sync::Arc; pub struct StubPlatform {} @@ -30,6 +33,7 @@ impl Default for StubPlatform { } } +#[async_trait] impl Platform for StubPlatform { fn name(&self) -> &'static str { "Stub" @@ -44,22 +48,13 @@ impl Platform for StubPlatform { Ok(()) } - fn create_services( - &self, - operations_service: Arc, - ) -> Result> { - // Add available services here - Ok(vec![ - ServiceEnum::Imu(ImuServiceServer::new(IMUServiceImpl::new(Arc::new( - StubIMU::new(operations_service.clone()), - )))), - ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( - Arc::new(StubActuator::new(operations_service.clone())), - ))), - ServiceEnum::ProcessManager(ProcessManagerServiceServer::new( - ProcessManagerServiceImpl::new(Arc::new(StubProcessManager::new())), - )), - ]) + fn create_services<'a>( + &'a self, + _operations_service: Arc, + ) -> Pin>> + Send + 'a>> { + Box::pin(async move { + Ok(vec![]) // or whatever the stub implementation should return + }) } fn shutdown(&mut self) -> eyre::Result<()> { From d94f8ffb1a7afba417f6348c84a77c17f8bf9763 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 3 Dec 2024 06:40:52 +0000 Subject: [PATCH 12/37] zeroing, change ids --- platforms/kbot/src/actuator.rs | 10 ++++++++++ platforms/kbot/src/lib.rs | 3 +-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 22066f0..730121e 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -115,6 +115,16 @@ impl Actuator for KBotActuator { } } + if let Some(zero_position) = config.zero_position { + if zero_position { + supervisor.zero(motor_id).await?; + } + } + + if let Some(new_actuator_id) = config.new_actuator_id { + supervisor.change_id(motor_id, new_actuator_id as u8).await?; + } + let success = result.is_ok(); let error = result.err().map(|e| KosError { code: ErrorCode::HardwareFailure as i32, diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 6c8a91b..275bd6b 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -11,7 +11,7 @@ pub use hexmove::*; pub use process_manager::*; use async_trait::async_trait; -use eyre::{Result, WrapErr}; +use eyre::WrapErr; use kos_core::hal::Operation; use kos_core::kos_proto::actuator::actuator_service_server::ActuatorServiceServer; use kos_core::kos_proto::process_manager::process_manager_service_server::ProcessManagerServiceServer; @@ -19,7 +19,6 @@ use kos_core::{ services::{ActuatorServiceImpl, OperationsServiceImpl, ProcessManagerServiceImpl}, Platform, ServiceEnum, }; -use std::collections::HashMap; use std::future::Future; use std::pin::Pin; use std::sync::Arc; From be57f3b3e7e61ed1a218a9b5a1f71be24974a275 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 01:32:34 +0000 Subject: [PATCH 13/37] actuator types --- platforms/kbot/Cargo.toml | 2 +- platforms/kbot/src/actuator.rs | 10 ++++++---- platforms/kbot/src/lib.rs | 31 +++++++++++++++++++++++++------ 3 files changed, 32 insertions(+), 11 deletions(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index b613239..75e599b 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = "0.1" +robstridev2 = { version = "0.1", path = "../../../robstride" } gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 730121e..7bf48be 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -22,9 +22,11 @@ impl KBotActuator { pub async fn new( _operations_service: Arc, ports: Vec<&str>, - motors_timeout: Duration, + actuator_timeout: Duration, + polling_interval: Duration, + desired_actuator_types: &[(u8, robstridev2::ActuatorType)], ) -> Result { - let mut supervisor = Supervisor::new(motors_timeout)?; + let mut supervisor = Supervisor::new(actuator_timeout)?; for port in ports.clone() { if port.starts_with("/dev/tty") { @@ -44,13 +46,13 @@ impl KBotActuator { let mut supervisor_runner = supervisor.clone_controller(); let _supervisor_handle = tokio::spawn(async move { - if let Err(e) = supervisor_runner.run().await { + if let Err(e) = supervisor_runner.run(polling_interval).await { tracing::error!("Supervisor task failed: {}", e); } }); for port in ports.clone() { - supervisor.scan_bus(0xFD, port).await?; + supervisor.scan_bus(0xFD, port, desired_actuator_types).await?; } Ok(KBotActuator { diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 275bd6b..155bbd7 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -5,6 +5,7 @@ mod process_manager; mod hexmove; pub use actuator::*; +pub use robstridev2::ActuatorType; #[cfg(target_os = "linux")] pub use hexmove::*; @@ -65,9 +66,21 @@ impl Platform for KbotPlatform { .wrap_err("Failed to initialize GStreamer process manager")?; let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04), + (1, ActuatorType::RobStride04), // Left Hip + (2, robstridev2::ActuatorType::RobStride03), // Left Knee + (3, robstridev2::ActuatorType::RobStride03), // Right Hip + (4, robstridev2::ActuatorType::RobStride04), // Right Knee + (5, robstridev2::ActuatorType::RobStride02), // Torso + ], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( @@ -79,9 +92,15 @@ impl Platform for KbotPlatform { ]) } else { let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04)], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( ActuatorServiceImpl::new(Arc::new(actuator)), From fa63742e64dba7d30be6b9663d133e5bfa5fed4a Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 01:54:00 +0000 Subject: [PATCH 14/37] k1 config --- platforms/kbot/src/lib.rs | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 155bbd7..32b96d8 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -71,12 +71,36 @@ impl Platform for KbotPlatform { vec!["can0"], Duration::from_secs(1), Duration::from_nanos(3_333_333), - &[(1, robstridev2::ActuatorType::RobStride04), - (1, ActuatorType::RobStride04), // Left Hip - (2, robstridev2::ActuatorType::RobStride03), // Left Knee - (3, robstridev2::ActuatorType::RobStride03), // Right Hip - (4, robstridev2::ActuatorType::RobStride04), // Right Knee - (5, robstridev2::ActuatorType::RobStride02), // Torso + &[ + // Right Leg + (10, ActuatorType::RobStride04), + (11, ActuatorType::RobStride03), + (12, ActuatorType::RobStride03), + (13, ActuatorType::RobStride04), + (14, ActuatorType::RobStride02), + + // Left Leg + (20, ActuatorType::RobStride04), + (21, ActuatorType::RobStride03), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride04), + (24, ActuatorType::RobStride02), + + // Right Arm + (30, ActuatorType::RobStride03), + (31, ActuatorType::RobStride03), + (32, ActuatorType::RobStride02), + (33, ActuatorType::RobStride02), + (34, ActuatorType::RobStride02), + (35, ActuatorType::RobStride00), + + // Left Arm + (40, ActuatorType::RobStride03), + (41, ActuatorType::RobStride03), + (42, ActuatorType::RobStride02), + (43, ActuatorType::RobStride02), + (44, ActuatorType::RobStride02), + (45, ActuatorType::RobStride00), ], ) .await From 273f3e17addfda11e192d1ae49bca00708e0f098 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 03:10:37 +0000 Subject: [PATCH 15/37] convert to proto units of measurement --- platforms/kbot/src/actuator.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 7bf48be..4505f6b 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -75,7 +75,7 @@ impl Actuator for KBotActuator { .position .map(|p| p.to_radians() as f32) .unwrap_or(0.0), - command.velocity.map(|v| v as f32).unwrap_or(0.0), + command.velocity.map(|v| v.to_radians() as f32).unwrap_or(0.0), command.torque.map(|t| t as f32).unwrap_or(0.0), ) .await; @@ -151,8 +151,8 @@ impl Actuator for KBotActuator { responses.push(ActuatorStateResponse { actuator_id: id, online: ts.elapsed().unwrap_or(Duration::from_secs(1)) < Duration::from_secs(1), - position: Some(feedback.angle as f64), - velocity: Some(feedback.velocity as f64), + position: Some(feedback.angle.to_degrees() as f64), + velocity: Some(feedback.velocity.to_degrees() as f64), torque: Some(feedback.torque as f64), temperature: Some(feedback.temperature as f64), voltage: None, From f29687bbd7093d114991698964d0dfdb53b6d1c5 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 06:07:50 +0000 Subject: [PATCH 16/37] fmt --- platforms/kbot/src/actuator.rs | 13 +++-- platforms/kbot/src/lib.rs | 89 ++++++++++++++++------------------ 2 files changed, 52 insertions(+), 50 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 4505f6b..41a2d0d 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -52,7 +52,9 @@ impl KBotActuator { }); for port in ports.clone() { - supervisor.scan_bus(0xFD, port, desired_actuator_types).await?; + supervisor + .scan_bus(0xFD, port, desired_actuator_types) + .await?; } Ok(KBotActuator { @@ -75,7 +77,10 @@ impl Actuator for KBotActuator { .position .map(|p| p.to_radians() as f32) .unwrap_or(0.0), - command.velocity.map(|v| v.to_radians() as f32).unwrap_or(0.0), + command + .velocity + .map(|v| v.to_radians() as f32) + .unwrap_or(0.0), command.torque.map(|t| t as f32).unwrap_or(0.0), ) .await; @@ -124,7 +129,9 @@ impl Actuator for KBotActuator { } if let Some(new_actuator_id) = config.new_actuator_id { - supervisor.change_id(motor_id, new_actuator_id as u8).await?; + supervisor + .change_id(motor_id, new_actuator_id as u8) + .await?; } let success = result.is_ok(); diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 32b96d8..92afb8d 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -65,46 +65,42 @@ impl Platform for KbotPlatform { KBotProcessManager::new(self.name().to_string(), self.serial()) .wrap_err("Failed to initialize GStreamer process manager")?; - let actuator = - KBotActuator::new( - operations_service, - vec!["can0"], - Duration::from_secs(1), - Duration::from_nanos(3_333_333), - &[ - // Right Leg - (10, ActuatorType::RobStride04), - (11, ActuatorType::RobStride03), - (12, ActuatorType::RobStride03), - (13, ActuatorType::RobStride04), - (14, ActuatorType::RobStride02), - - // Left Leg - (20, ActuatorType::RobStride04), - (21, ActuatorType::RobStride03), - (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride04), - (24, ActuatorType::RobStride02), - - // Right Arm - (30, ActuatorType::RobStride03), - (31, ActuatorType::RobStride03), - (32, ActuatorType::RobStride02), - (33, ActuatorType::RobStride02), - (34, ActuatorType::RobStride02), - (35, ActuatorType::RobStride00), - - // Left Arm - (40, ActuatorType::RobStride03), - (41, ActuatorType::RobStride03), - (42, ActuatorType::RobStride02), - (43, ActuatorType::RobStride02), - (44, ActuatorType::RobStride02), - (45, ActuatorType::RobStride00), + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[ + // Right Leg + (10, ActuatorType::RobStride04), + (11, ActuatorType::RobStride03), + (12, ActuatorType::RobStride03), + (13, ActuatorType::RobStride04), + (14, ActuatorType::RobStride02), + // Left Leg + (20, ActuatorType::RobStride04), + (21, ActuatorType::RobStride03), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride04), + (24, ActuatorType::RobStride02), + // Right Arm + (30, ActuatorType::RobStride03), + (31, ActuatorType::RobStride03), + (32, ActuatorType::RobStride02), + (33, ActuatorType::RobStride02), + (34, ActuatorType::RobStride02), + (35, ActuatorType::RobStride00), + // Left Arm + (40, ActuatorType::RobStride03), + (41, ActuatorType::RobStride03), + (42, ActuatorType::RobStride02), + (43, ActuatorType::RobStride02), + (44, ActuatorType::RobStride02), + (45, ActuatorType::RobStride00), ], ) .await - .wrap_err("Failed to create actuator")?; + .wrap_err("Failed to create actuator")?; Ok(vec![ ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( @@ -115,16 +111,15 @@ impl Platform for KbotPlatform { )), ]) } else { - let actuator = - KBotActuator::new( - operations_service, - vec!["can0"], - Duration::from_secs(1), - Duration::from_nanos(3_333_333), - &[(1, robstridev2::ActuatorType::RobStride04)], - ) - .await - .wrap_err("Failed to create actuator")?; + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04)], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( ActuatorServiceImpl::new(Arc::new(actuator)), From c59bd751616c4ab4336392b4d99af1db7ec60e6e Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 06:08:18 +0000 Subject: [PATCH 17/37] deps fix --- platforms/kbot/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index 75e599b..71b61f4 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = { version = "0.1", path = "../../../robstride" } +robstridev2 = "0.2" gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" From 068fcadc0794fed3f68604c37179f946ad86ce8b Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 4 Dec 2024 06:42:13 +0000 Subject: [PATCH 18/37] local dependency --- platforms/kbot/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index 71b61f4..15ccba1 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = "0.2" +robstridev2 = {version = "0.2.1", path = "../../robstridev2"} gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" @@ -21,4 +21,4 @@ uuid = "1" tokio = { version = "1", features = ["full"] } [target.'cfg(target_os = "linux")'.dependencies] imu = ">=0.1.6" -chrono = "0.4" \ No newline at end of file +chrono = "0.4" From f75a6945148e5ba6836e9caed0ae78f516e9923b Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 4 Dec 2024 06:46:23 +0000 Subject: [PATCH 19/37] fix ids --- platforms/kbot/src/lib.rs | 49 +++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 92afb8d..3405c08 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -67,36 +67,41 @@ impl Platform for KbotPlatform { let actuator = KBotActuator::new( operations_service, - vec!["can0"], + vec![ + "/dev/ttyCH341USB0", + "/dev/ttyCH341USB1", + "/dev/ttyCH341USB2", + "/dev/ttyCH341USB3", + ], Duration::from_secs(1), Duration::from_nanos(3_333_333), &[ - // Right Leg - (10, ActuatorType::RobStride04), + // Left Arm + (1, ActuatorType::RobStride03), + (2, ActuatorType::RobStride03), + (3, ActuatorType::RobStride02), + (4, ActuatorType::RobStride02), + (5, ActuatorType::RobStride02), + (6, ActuatorType::RobStride00), + // Right Arm (11, ActuatorType::RobStride03), (12, ActuatorType::RobStride03), - (13, ActuatorType::RobStride04), + (13, ActuatorType::RobStride02), (14, ActuatorType::RobStride02), + (15, ActuatorType::RobStride02), + (16, ActuatorType::RobStride00), // Left Leg - (20, ActuatorType::RobStride04), - (21, ActuatorType::RobStride03), + (21, ActuatorType::RobStride04), (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride04), - (24, ActuatorType::RobStride02), - // Right Arm - (30, ActuatorType::RobStride03), - (31, ActuatorType::RobStride03), - (32, ActuatorType::RobStride02), - (33, ActuatorType::RobStride02), - (34, ActuatorType::RobStride02), - (35, ActuatorType::RobStride00), - // Left Arm - (40, ActuatorType::RobStride03), - (41, ActuatorType::RobStride03), - (42, ActuatorType::RobStride02), - (43, ActuatorType::RobStride02), - (44, ActuatorType::RobStride02), - (45, ActuatorType::RobStride00), + (23, ActuatorType::RobStride03), + (24, ActuatorType::RobStride04), + (25, ActuatorType::RobStride02), + // Right Leg + (31, ActuatorType::RobStride04), + (32, ActuatorType::RobStride03), + (33, ActuatorType::RobStride03), + (34, ActuatorType::RobStride04), + (35, ActuatorType::RobStride02), ], ) .await From 069e38b4a6a9e1f5ebadca7aa7bee135f8fe17d9 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 4 Dec 2024 09:47:14 +0000 Subject: [PATCH 20/37] test --- platforms/kbot/src/lib.rs | 40 ++++++++++++++++++++++----------------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 3405c08..62b82b7 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -61,6 +61,8 @@ impl Platform for KbotPlatform { ) -> Pin>> + Send + 'a>> { Box::pin(async move { if cfg!(target_os = "linux") { + tracing::debug!("Initializing KBot services for Linux"); + let process_manager = KBotProcessManager::new(self.name().to_string(), self.serial()) .wrap_err("Failed to initialize GStreamer process manager")?; @@ -68,13 +70,17 @@ impl Platform for KbotPlatform { let actuator = KBotActuator::new( operations_service, vec![ - "/dev/ttyCH341USB0", - "/dev/ttyCH341USB1", - "/dev/ttyCH341USB2", - "/dev/ttyCH341USB3", + // "/dev/ttyCH341USB0", + // "/dev/ttyCH341USB1", + // "/dev/ttyCH341USB2", + // "/dev/ttyCH341USB3", + "can0", + "can1", + "can2", ], Duration::from_secs(1), - Duration::from_nanos(3_333_333), + // Duration::from_nanos(3_333_333), + Duration::from_millis(10), &[ // Left Arm (1, ActuatorType::RobStride03), @@ -90,18 +96,18 @@ impl Platform for KbotPlatform { (14, ActuatorType::RobStride02), (15, ActuatorType::RobStride02), (16, ActuatorType::RobStride00), - // Left Leg - (21, ActuatorType::RobStride04), - (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride03), - (24, ActuatorType::RobStride04), - (25, ActuatorType::RobStride02), - // Right Leg - (31, ActuatorType::RobStride04), - (32, ActuatorType::RobStride03), - (33, ActuatorType::RobStride03), - (34, ActuatorType::RobStride04), - (35, ActuatorType::RobStride02), + // // Left Leg + // (21, ActuatorType::RobStride04), + // (22, ActuatorType::RobStride03), + // (23, ActuatorType::RobStride03), + // (24, ActuatorType::RobStride04), + // (25, ActuatorType::RobStride02), + // // Right Leg + // (31, ActuatorType::RobStride04), + // (32, ActuatorType::RobStride03), + // (33, ActuatorType::RobStride03), + // (34, ActuatorType::RobStride04), + // (35, ActuatorType::RobStride02), ], ) .await From 71718075fed58ef3891b36fc00b5da7d31525be9 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 4 Dec 2024 12:55:13 +0000 Subject: [PATCH 21/37] add warning if configured motors not found --- platforms/kbot/src/actuator.rs | 19 ++++++++++++++++++- platforms/kbot/src/lib.rs | 26 +++++++++++++------------- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 41a2d0d..4d3df88 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -27,6 +27,7 @@ impl KBotActuator { desired_actuator_types: &[(u8, robstridev2::ActuatorType)], ) -> Result { let mut supervisor = Supervisor::new(actuator_timeout)?; + let mut found_motors = vec![false; desired_actuator_types.len()]; for port in ports.clone() { if port.starts_with("/dev/tty") { @@ -52,9 +53,25 @@ impl KBotActuator { }); for port in ports.clone() { - supervisor + let discovered_ids = supervisor .scan_bus(0xFD, port, desired_actuator_types) .await?; + + for (idx, (motor_id, _)) in desired_actuator_types.iter().enumerate() { + if discovered_ids.contains(motor_id) { + found_motors[idx] = true; + } + } + } + + for (idx, (motor_id, motor_type)) in desired_actuator_types.iter().enumerate() { + if !found_motors[idx] { + tracing::warn!( + "Configured motor not found - ID: {}, Type: {:?}", + motor_id, + motor_type + ); + } } Ok(KBotActuator { diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 62b82b7..a7811a5 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -80,7 +80,7 @@ impl Platform for KbotPlatform { ], Duration::from_secs(1), // Duration::from_nanos(3_333_333), - Duration::from_millis(10), + Duration::from_millis(5), &[ // Left Arm (1, ActuatorType::RobStride03), @@ -96,18 +96,18 @@ impl Platform for KbotPlatform { (14, ActuatorType::RobStride02), (15, ActuatorType::RobStride02), (16, ActuatorType::RobStride00), - // // Left Leg - // (21, ActuatorType::RobStride04), - // (22, ActuatorType::RobStride03), - // (23, ActuatorType::RobStride03), - // (24, ActuatorType::RobStride04), - // (25, ActuatorType::RobStride02), - // // Right Leg - // (31, ActuatorType::RobStride04), - // (32, ActuatorType::RobStride03), - // (33, ActuatorType::RobStride03), - // (34, ActuatorType::RobStride04), - // (35, ActuatorType::RobStride02), + // Left Leg + (21, ActuatorType::RobStride04), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride03), + (24, ActuatorType::RobStride04), + (25, ActuatorType::RobStride02), + // Right Leg + (31, ActuatorType::RobStride04), + (32, ActuatorType::RobStride03), + (33, ActuatorType::RobStride03), + (34, ActuatorType::RobStride04), + (35, ActuatorType::RobStride02), ], ) .await From 91fb9923f7606f1f4ad6b31c7ee5f0378c1aff48 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 4 Dec 2024 16:46:55 +0000 Subject: [PATCH 22/37] nums to fix it --- platforms/kbot/src/actuator.rs | 2 +- platforms/kbot/src/lib.rs | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 4d3df88..686149d 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -123,7 +123,7 @@ impl Actuator for KBotActuator { let control_config = ControlConfig { kp: config.kp.unwrap_or(0.0) as f32, kd: config.kd.unwrap_or(0.0) as f32, - max_torque: Some(2.0), + max_torque: Some(4.0), max_velocity: Some(5.0), max_current: Some(10.0), }; diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index a7811a5..2c9dd32 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -80,7 +80,7 @@ impl Platform for KbotPlatform { ], Duration::from_secs(1), // Duration::from_nanos(3_333_333), - Duration::from_millis(5), + Duration::from_millis(7), &[ // Left Arm (1, ActuatorType::RobStride03), @@ -88,14 +88,14 @@ impl Platform for KbotPlatform { (3, ActuatorType::RobStride02), (4, ActuatorType::RobStride02), (5, ActuatorType::RobStride02), - (6, ActuatorType::RobStride00), + // (6, ActuatorType::RobStride00), // Right Arm (11, ActuatorType::RobStride03), (12, ActuatorType::RobStride03), (13, ActuatorType::RobStride02), (14, ActuatorType::RobStride02), (15, ActuatorType::RobStride02), - (16, ActuatorType::RobStride00), + // (16, ActuatorType::RobStride00), // Left Leg (21, ActuatorType::RobStride04), (22, ActuatorType::RobStride03), From abff83d8e63259a0c2865f5c3792a84f86d4142d Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 14:38:59 -0800 Subject: [PATCH 23/37] krec fixes --- daemon/src/main.rs | 3 +- kos_core/src/services/krec_logger.rs | 52 ++++++++++++++++++------- pykos/pykos/services/process_manager.py | 6 ++- 3 files changed, 45 insertions(+), 16 deletions(-) diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 0513a30..7ead2ef 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -74,7 +74,8 @@ async fn main() -> Result<()> { .add_directive("rumqttc=error".parse().unwrap()) .add_directive("kos_core::telemetry=error".parse().unwrap()) .add_directive("polling=error".parse().unwrap()) - .add_directive("async_io=error".parse().unwrap()), + .add_directive("async_io=error".parse().unwrap()) + .add_directive("krec=error".parse().unwrap()), ) .init(); diff --git a/kos_core/src/services/krec_logger.rs b/kos_core/src/services/krec_logger.rs index 0956cd0..c7eca70 100644 --- a/kos_core/src/services/krec_logger.rs +++ b/kos_core/src/services/krec_logger.rs @@ -29,6 +29,26 @@ struct ActuatorCommandItem { torque: Option, } +#[derive(Deserialize, Debug)] +struct ActuatorStateData { + actuator_id: u32, + online: bool, + position: Option, + velocity: Option, + torque: Option, + temperature: Option, + voltage: Option, + current: Option, +} + +#[derive(Deserialize, Debug)] +struct ActuatorStateList { + frame_number: u64, + video_timestamp: u64, + inference_step: u64, + data: Vec, +} + pub struct TelemetryLogger { krec: Arc>, _mqtt_client: AsyncClient, @@ -158,19 +178,24 @@ impl TelemetryLogger { tracing::error!("Failed to decode QuaternionResponse {:?}", payload); } } else if topic.contains("/actuator/state") { - if let Ok(state) = ActuatorStateResponse::decode(payload.as_ref()) { - frame.actuator_states.push(ActuatorState { - actuator_id: state.actuator_id, - online: state.online, - position: state.position, - velocity: state.velocity, - torque: state.torque, - temperature: state.temperature, - voltage: state.voltage, - current: state.current, - }); - } else { - tracing::error!("Failed to decode ActuatorStateResponse {:?}", payload); + match serde_json::from_slice::(&payload) { + Ok(state_list) => { + for state in state_list.data { + frame.actuator_states.push(ActuatorState { + actuator_id: state.actuator_id, + online: state.online, + position: state.position, + velocity: state.velocity, + torque: state.torque, + temperature: state.temperature, + voltage: state.voltage, + current: state.current, + }); + } + } + Err(e) => { + tracing::error!("Failed to parse actuator state JSON: {:?}", e); + } } } else if topic.contains("/actuator/command") { match serde_json::from_slice::(&payload) { @@ -187,7 +212,6 @@ impl TelemetryLogger { torque: item.torque.unwrap_or_default() as f32, }); } - tracing::debug!("Parsed actuator command: {:?}", frame); } Err(e) => { tracing::error!("Failed to parse actuator command JSON: {:?}", e); diff --git a/pykos/pykos/services/process_manager.py b/pykos/pykos/services/process_manager.py index d83f9b4..c0a35c6 100644 --- a/pykos/pykos/services/process_manager.py +++ b/pykos/pykos/services/process_manager.py @@ -14,14 +14,18 @@ class ProcessManagerServiceClient: def __init__(self, channel: grpc.Channel) -> None: self.stub = process_manager_pb2_grpc.ProcessManagerServiceStub(channel) - def start_kclip(self, request: KClipStartRequest) -> Tuple[Optional[str], Optional[Error]]: + def start_kclip(self, action: str) -> Tuple[Optional[str], Optional[Error]]: """Start KClip recording. + Args: + action: The action string for the KClip request + Returns: Tuple containing: - clip_uuid (str): UUID of the started clip, if successful - error (Error): Error details if the operation failed """ + request = KClipStartRequest(action=action) response = self.stub.StartKClip(request) return response.clip_uuid, response.error if response.HasField("error") else None From c9ef7640c84036d7c36a1becfcf0f5b88e0fde83 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 15:20:26 -0800 Subject: [PATCH 24/37] kclip fixes --- kos_core/Cargo.toml | 2 +- kos_core/src/services/krec_logger.rs | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/kos_core/Cargo.toml b/kos_core/Cargo.toml index 5ff0b5e..665615a 100644 --- a/kos_core/Cargo.toml +++ b/kos_core/Cargo.toml @@ -26,7 +26,7 @@ eyre = "0.6" hyper = "0.14" tracing = "0.1" lazy_static = "1.4" -krec = "0.1" +krec = "0.2" [build-dependencies] tonic-build = "0.12" diff --git a/kos_core/src/services/krec_logger.rs b/kos_core/src/services/krec_logger.rs index c7eca70..a4347e5 100644 --- a/kos_core/src/services/krec_logger.rs +++ b/kos_core/src/services/krec_logger.rs @@ -202,7 +202,12 @@ impl TelemetryLogger { Ok(command_data) => { frame.inference_step = command_data.inference_step; frame.video_timestamp = command_data.video_timestamp; - frame.frame_number = command_data.frame_number; + frame.video_frame_number = command_data.frame_number; + frame.real_timestamp = std::time::SystemTime::now() + .duration_since(std::time::UNIX_EPOCH) + .unwrap_or_default() + .as_nanos() + as u64; for item in command_data.data { frame.actuator_commands.push(ActuatorCommand { From 59455755c5409cf1485b2ad85020963b251936e8 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Thu, 5 Dec 2024 05:38:12 +0000 Subject: [PATCH 25/37] config torque --- platforms/kbot/src/actuator.rs | 2 +- platforms/kbot/src/lib.rs | 26 +++++++++++++------------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 686149d..0c35ec8 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -123,7 +123,7 @@ impl Actuator for KBotActuator { let control_config = ControlConfig { kp: config.kp.unwrap_or(0.0) as f32, kd: config.kd.unwrap_or(0.0) as f32, - max_torque: Some(4.0), + max_torque: Some(config.max_torque.unwrap_or(2.0) as f32), max_velocity: Some(5.0), max_current: Some(10.0), }; diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 2c9dd32..bd1faaa 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -83,31 +83,31 @@ impl Platform for KbotPlatform { Duration::from_millis(7), &[ // Left Arm - (1, ActuatorType::RobStride03), - (2, ActuatorType::RobStride03), - (3, ActuatorType::RobStride02), - (4, ActuatorType::RobStride02), - (5, ActuatorType::RobStride02), - // (6, ActuatorType::RobStride00), - // Right Arm (11, ActuatorType::RobStride03), (12, ActuatorType::RobStride03), (13, ActuatorType::RobStride02), (14, ActuatorType::RobStride02), (15, ActuatorType::RobStride02), - // (16, ActuatorType::RobStride00), - // Left Leg - (21, ActuatorType::RobStride04), + (16, ActuatorType::RobStride00), + // Right Arm + (21, ActuatorType::RobStride03), (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride03), - (24, ActuatorType::RobStride04), + (23, ActuatorType::RobStride02), + (24, ActuatorType::RobStride02), (25, ActuatorType::RobStride02), - // Right Leg + (26, ActuatorType::RobStride00), + // Left Leg (31, ActuatorType::RobStride04), (32, ActuatorType::RobStride03), (33, ActuatorType::RobStride03), (34, ActuatorType::RobStride04), (35, ActuatorType::RobStride02), + // Right Leg + (41, ActuatorType::RobStride04), + (42, ActuatorType::RobStride03), + (43, ActuatorType::RobStride03), + (44, ActuatorType::RobStride04), + (45, ActuatorType::RobStride02), ], ) .await From ea501eefa8367cf64ff6404808390376afdbae6b Mon Sep 17 00:00:00 2001 From: WT-MM Date: Fri, 6 Dec 2024 02:10:43 +0000 Subject: [PATCH 26/37] trace logging --- .gitignore | 4 ++ daemon/Cargo.toml | 4 ++ daemon/src/main.rs | 83 +++++++++++++++++++++++---- platforms/kbot/src/process_manager.rs | 8 +-- 4 files changed, 85 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index 7ddd7f5..37e353c 100644 --- a/.gitignore +++ b/.gitignore @@ -31,3 +31,7 @@ build/ dist/ *.so out*/ + +# Dev +*.patch +robstridev2/ diff --git a/daemon/Cargo.toml b/daemon/Cargo.toml index eaad24f..3070aa2 100644 --- a/daemon/Cargo.toml +++ b/daemon/Cargo.toml @@ -20,6 +20,10 @@ tower = "0.5" gstreamer = "0.23" gstreamer-base = "0.23" glib = "0.17" +tracing-appender = "0.2" +clap = { version = "4.0", features = ["derive"] } +chrono = "0.4" +directories = "5.0" kos-sim = { version = "0.1.0", path = "../platforms/sim", optional = true } kos-stub = { version = "0.1.0", path = "../platforms/stub", optional = true } diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 7ead2ef..6d04efe 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -14,6 +14,14 @@ use tokio::sync::Mutex; use tonic::transport::Server; use tracing::{debug, error, info}; use tracing_subscriber::filter::EnvFilter; +use tracing_appender::rolling::RollingFileAppender; +use tracing_appender::non_blocking::NonBlocking; +use tracing_subscriber::Layer; +use tracing_subscriber::prelude::*; +use chrono::Local; +use clap::Parser; +use directories::BaseDirs; +use std::path::PathBuf; #[cfg(not(any(feature = "kos-sim", feature = "kos-zeroth-01", feature = "kos-kbot")))] use kos_stub::StubPlatform as PlatformImpl; @@ -27,6 +35,14 @@ use kos_zeroth_01::Zeroth01Platform as PlatformImpl; #[cfg(feature = "kos-kbot")] use kos_kbot::KbotPlatform as PlatformImpl; +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// Enable file logging + #[arg(long, default_value_t = false)] + log: bool, +} + fn add_service_to_router( router: tonic::transport::server::Router, service: ServiceEnum, @@ -63,11 +79,22 @@ async fn run_server( Ok(()) } +struct DaemonState { + _guard: Option, + platform: PlatformImpl, +} + #[tokio::main] async fn main() -> Result<()> { - // logging - tracing_subscriber::fmt() - .with_env_filter( + let args = Args::parse(); + + // tracing + let subscriber = tracing_subscriber::registry(); + + // Always add stdout layer + let stdout_layer = tracing_subscriber::fmt::layer() + .with_writer(std::io::stdout) + .with_filter( EnvFilter::from_default_env() .add_directive("h2=error".parse().unwrap()) .add_directive("grpc=error".parse().unwrap()) @@ -76,14 +103,50 @@ async fn main() -> Result<()> { .add_directive("polling=error".parse().unwrap()) .add_directive("async_io=error".parse().unwrap()) .add_directive("krec=error".parse().unwrap()), - ) - .init(); - - let mut platform = PlatformImpl::new(); + ); + + let subscriber = subscriber.with(stdout_layer); + + let guard = if args.log { + let log_dir = if let Some(base_dirs) = BaseDirs::new() { + base_dirs.data_local_dir() + .join("kos") + .join("logs") + } else { + PathBuf::from("~/.local/share/kos/logs") + }; + + std::fs::create_dir_all(&log_dir)?; + + let timestamp = Local::now().format("%Y%m%d_%H%M%S"); + let log_name = format!("kos-daemon_{}.log", timestamp); + + let file_appender = RollingFileAppender::new( + tracing_appender::rolling::Rotation::NEVER, + log_dir, + &log_name, + ); + let (non_blocking, guard) = tracing_appender::non_blocking(file_appender); + + let file_layer = tracing_subscriber::fmt::layer() + .with_writer(non_blocking) + .with_filter(tracing_subscriber::filter::LevelFilter::TRACE); + + subscriber.with(file_layer).init(); + Some(guard) + } else { + subscriber.init(); + None + }; + + let mut state = DaemonState { + _guard: guard, + platform: PlatformImpl::new(), + }; // telemetry Telemetry::initialize( - format!("{}-{}", platform.name(), platform.serial()).as_str(), + format!("{}-{}", state.platform.name(), state.platform.serial()).as_str(), "localhost", 1883, ) @@ -92,9 +155,9 @@ async fn main() -> Result<()> { let operations_store = Arc::new(Mutex::new(HashMap::new())); let operations_service = Arc::new(OperationsServiceImpl::new(operations_store)); - platform.initialize(operations_service.clone())?; + state.platform.initialize(operations_service.clone())?; - if let Err(e) = run_server(&platform, operations_service).await { + if let Err(e) = run_server(&state.platform, operations_service).await { error!("Server error: {:?}", e); std::process::exit(1); } diff --git a/platforms/kbot/src/process_manager.rs b/platforms/kbot/src/process_manager.rs index 15751ef..85e0077 100644 --- a/platforms/kbot/src/process_manager.rs +++ b/platforms/kbot/src/process_manager.rs @@ -245,10 +245,10 @@ impl ProcessManager for KBotProcessManager { *kclip_uuid = Some(new_uuid.clone()); drop(kclip_uuid); - let (pipeline, _sink) = Self::create_pipeline(&video_path)?; + // let (pipeline, _sink) = Self::create_pipeline(&video_path)?; - // Start the pipeline - pipeline.set_state(gst::State::Playing)?; + // // Start the pipeline + // pipeline.set_state(gst::State::Playing)?; // Start telemetry logger let logger = TelemetryLogger::new( @@ -265,7 +265,7 @@ impl ProcessManager for KBotProcessManager { drop(telemetry_logger); let mut pipeline_guard = self.pipeline.lock().await; - *pipeline_guard = Some(pipeline); + *pipeline_guard = None; Ok(KClipStartResponse { clip_uuid: Some(new_uuid), From 0d33ba2783cfe0a98da5380648544ff88e2375f1 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Fri, 6 Dec 2024 02:55:54 +0000 Subject: [PATCH 27/37] compression --- README.md | 5 ++ daemon/Cargo.toml | 1 + daemon/src/main.rs | 122 ++++++++++++++++++++++++++++++++++++++++----- 3 files changed, 116 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 3c1f88c..aefc051 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,11 @@ You can specify logging levels for individual modules by adding `module_name=log RUST_LOG=debug,krec=warn cargo run --features stub ``` +To save trace logs to a file, pass the `--log` flag: +```bash +cargo run --features stub -- --log +``` + ## Contributing - Use `cargo fmt --all` to format the code. - Use `cargo clippy` to check for lint errors. diff --git a/daemon/Cargo.toml b/daemon/Cargo.toml index 3070aa2..fd6d01c 100644 --- a/daemon/Cargo.toml +++ b/daemon/Cargo.toml @@ -24,6 +24,7 @@ tracing-appender = "0.2" clap = { version = "4.0", features = ["derive"] } chrono = "0.4" directories = "5.0" +flate2 = "1.0" kos-sim = { version = "0.1.0", path = "../platforms/sim", optional = true } kos-stub = { version = "0.1.0", path = "../platforms/stub", optional = true } diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 6d04efe..11831f5 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -14,14 +14,17 @@ use tokio::sync::Mutex; use tonic::transport::Server; use tracing::{debug, error, info}; use tracing_subscriber::filter::EnvFilter; -use tracing_appender::rolling::RollingFileAppender; -use tracing_appender::non_blocking::NonBlocking; use tracing_subscriber::Layer; use tracing_subscriber::prelude::*; use chrono::Local; use clap::Parser; use directories::BaseDirs; use std::path::PathBuf; +use std::io::{self, Write, BufWriter}; +use flate2::write::GzEncoder; +use flate2::Compression; +use std::fs::File; +use tokio::signal; #[cfg(not(any(feature = "kos-sim", feature = "kos-zeroth-01", feature = "kos-kbot")))] use kos_stub::StubPlatform as PlatformImpl; @@ -84,6 +87,81 @@ struct DaemonState { platform: PlatformImpl, } +struct CompressedWriter { + encoder: Option>>, + path: PathBuf, +} + +impl CompressedWriter { + fn new(path: impl AsRef) -> io::Result { + let file = File::create(path.as_ref())?; + let buffered = BufWriter::new(file); + Ok(Self { + encoder: Some(GzEncoder::new(buffered, Compression::new(6))), + path: path.as_ref().to_path_buf(), + }) + } + + fn sync(&mut self) -> io::Result<()> { + if let Some(encoder) = &mut self.encoder { + encoder.flush()?; + let buf_writer = encoder.get_mut(); + buf_writer.flush()?; + let file = buf_writer.get_mut(); + file.sync_all()?; + } + Ok(()) + } + + fn finalize(&mut self) -> io::Result<()> { + info!("Finalizing compressed log {}", self.path.display()); + if let Some(encoder) = self.encoder.take() { + // Finish the compression + let mut buf_writer = encoder.finish()?; + // Flush the buffer + buf_writer.flush()?; + info!("Flushed compressed log {}", self.path.display()); + // Sync to disk + buf_writer.get_mut().sync_all()?; + } + Ok(()) + } +} + +impl Write for CompressedWriter { + fn write(&mut self, buf: &[u8]) -> io::Result { + if let Some(encoder) = &mut self.encoder { + match encoder.write(buf) { + Ok(size) => { + if size > 0 && buf.contains(&b'\n') { + self.sync()?; + } + Ok(size) + } + Err(e) => { + error!("Failed to write to compressed log {}: {}", self.path.display(), e); + Err(e) + } + } + } else { + error!("Attempted to write to finalized log {}", self.path.display()); + Err(io::Error::new(io::ErrorKind::Other, "Writer has been finalized")) + } + } + + fn flush(&mut self) -> io::Result<()> { + self.sync() + } +} + +impl Drop for CompressedWriter { + fn drop(&mut self) { + if let Err(e) = self.finalize() { + error!("Failed to finalize compressed log {}: {}", self.path.display(), e); + } + } +} + #[tokio::main] async fn main() -> Result<()> { let args = Args::parse(); @@ -119,16 +197,20 @@ async fn main() -> Result<()> { std::fs::create_dir_all(&log_dir)?; let timestamp = Local::now().format("%Y%m%d_%H%M%S"); - let log_name = format!("kos-daemon_{}.log", timestamp); + let temp_name = format!("kos-daemon_{}.log", timestamp); + let final_name = format!("kos-daemon_{}.log.gz", timestamp); + let log_path = log_dir.join(&final_name); - let file_appender = RollingFileAppender::new( - tracing_appender::rolling::Rotation::NEVER, - log_dir, - &log_name, - ); - let (non_blocking, guard) = tracing_appender::non_blocking(file_appender); + info!("Writing compressed logs to: {}", log_path.display()); + + let compressed_writer = CompressedWriter::new(&log_path)?; + let (non_blocking, guard) = tracing_appender::non_blocking(compressed_writer); let file_layer = tracing_subscriber::fmt::layer() + .with_thread_ids(true) + .with_target(true) + .with_file(true) + .with_line_number(true) .with_writer(non_blocking) .with_filter(tracing_subscriber::filter::LevelFilter::TRACE); @@ -144,6 +226,15 @@ async fn main() -> Result<()> { platform: PlatformImpl::new(), }; + // Setup signal handler + let (shutdown_tx, mut shutdown_rx) = tokio::sync::oneshot::channel(); + + tokio::spawn(async move { + if let Ok(()) = signal::ctrl_c().await { + let _ = shutdown_tx.send(()); + } + }); + // telemetry Telemetry::initialize( format!("{}-{}", state.platform.name(), state.platform.serial()).as_str(), @@ -157,9 +248,16 @@ async fn main() -> Result<()> { state.platform.initialize(operations_service.clone())?; - if let Err(e) = run_server(&state.platform, operations_service).await { - error!("Server error: {:?}", e); - std::process::exit(1); + tokio::select! { + res = run_server(&state.platform, operations_service) => { + if let Err(e) = res { + error!("Server error: {:?}", e); + std::process::exit(1); + } + } + _ = shutdown_rx => { + info!("Received shutdown signal, cleaning up..."); + } } Ok(()) From 034ed01be0663237b1aa428facbeccee05ba3355 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Fri, 6 Dec 2024 02:59:23 +0000 Subject: [PATCH 28/37] fmt --- daemon/src/main.rs | 53 +++++++++++++++++++++------------- platforms/kbot/src/actuator.rs | 2 +- platforms/kbot/src/lib.rs | 6 ++-- 3 files changed, 36 insertions(+), 25 deletions(-) diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 11831f5..b34bb22 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -2,29 +2,29 @@ // This will run the gRPC server and, if applicable, a runtime loop // (e.g., actuator polling, loaded model inference). +use chrono::Local; +use clap::Parser; +use directories::BaseDirs; use eyre::Result; +use flate2::write::GzEncoder; +use flate2::Compression; use kos_core::google_proto::longrunning::operations_server::OperationsServer; use kos_core::services::OperationsServiceImpl; use kos_core::telemetry::Telemetry; use kos_core::Platform; use kos_core::ServiceEnum; use std::collections::HashMap; +use std::fs::File; +use std::io::{self, BufWriter, Write}; +use std::path::PathBuf; use std::sync::Arc; +use tokio::signal; use tokio::sync::Mutex; use tonic::transport::Server; use tracing::{debug, error, info}; use tracing_subscriber::filter::EnvFilter; -use tracing_subscriber::Layer; use tracing_subscriber::prelude::*; -use chrono::Local; -use clap::Parser; -use directories::BaseDirs; -use std::path::PathBuf; -use std::io::{self, Write, BufWriter}; -use flate2::write::GzEncoder; -use flate2::Compression; -use std::fs::File; -use tokio::signal; +use tracing_subscriber::Layer; #[cfg(not(any(feature = "kos-sim", feature = "kos-zeroth-01", feature = "kos-kbot")))] use kos_stub::StubPlatform as PlatformImpl; @@ -92,6 +92,7 @@ struct CompressedWriter { path: PathBuf, } +// TODO: The encoder doesn't close properly, so this needs to be fixed later. impl CompressedWriter { fn new(path: impl AsRef) -> io::Result { let file = File::create(path.as_ref())?; @@ -139,13 +140,23 @@ impl Write for CompressedWriter { Ok(size) } Err(e) => { - error!("Failed to write to compressed log {}: {}", self.path.display(), e); + error!( + "Failed to write to compressed log {}: {}", + self.path.display(), + e + ); Err(e) } } } else { - error!("Attempted to write to finalized log {}", self.path.display()); - Err(io::Error::new(io::ErrorKind::Other, "Writer has been finalized")) + error!( + "Attempted to write to finalized log {}", + self.path.display() + ); + Err(io::Error::new( + io::ErrorKind::Other, + "Writer has been finalized", + )) } } @@ -157,7 +168,11 @@ impl Write for CompressedWriter { impl Drop for CompressedWriter { fn drop(&mut self) { if let Err(e) = self.finalize() { - error!("Failed to finalize compressed log {}: {}", self.path.display(), e); + error!( + "Failed to finalize compressed log {}: {}", + self.path.display(), + e + ); } } } @@ -168,7 +183,7 @@ async fn main() -> Result<()> { // tracing let subscriber = tracing_subscriber::registry(); - + // Always add stdout layer let stdout_layer = tracing_subscriber::fmt::layer() .with_writer(std::io::stdout) @@ -187,9 +202,7 @@ async fn main() -> Result<()> { let guard = if args.log { let log_dir = if let Some(base_dirs) = BaseDirs::new() { - base_dirs.data_local_dir() - .join("kos") - .join("logs") + base_dirs.data_local_dir().join("kos").join("logs") } else { PathBuf::from("~/.local/share/kos/logs") }; @@ -202,7 +215,7 @@ async fn main() -> Result<()> { let log_path = log_dir.join(&final_name); info!("Writing compressed logs to: {}", log_path.display()); - + let compressed_writer = CompressedWriter::new(&log_path)?; let (non_blocking, guard) = tracing_appender::non_blocking(compressed_writer); @@ -228,7 +241,7 @@ async fn main() -> Result<()> { // Setup signal handler let (shutdown_tx, mut shutdown_rx) = tokio::sync::oneshot::channel(); - + tokio::spawn(async move { if let Ok(()) = signal::ctrl_c().await { let _ = shutdown_tx.send(()); diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 0c35ec8..752d039 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -56,7 +56,7 @@ impl KBotActuator { let discovered_ids = supervisor .scan_bus(0xFD, port, desired_actuator_types) .await?; - + for (idx, (motor_id, _)) in desired_actuator_types.iter().enumerate() { if discovered_ids.contains(motor_id) { found_motors[idx] = true; diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index bd1faaa..7113202 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -62,7 +62,7 @@ impl Platform for KbotPlatform { Box::pin(async move { if cfg!(target_os = "linux") { tracing::debug!("Initializing KBot services for Linux"); - + let process_manager = KBotProcessManager::new(self.name().to_string(), self.serial()) .wrap_err("Failed to initialize GStreamer process manager")?; @@ -74,9 +74,7 @@ impl Platform for KbotPlatform { // "/dev/ttyCH341USB1", // "/dev/ttyCH341USB2", // "/dev/ttyCH341USB3", - "can0", - "can1", - "can2", + "can0", "can1", "can2", ], Duration::from_secs(1), // Duration::from_nanos(3_333_333), From 9510acf33da7171da56e6cfa37272d16c81ad73d Mon Sep 17 00:00:00 2001 From: WT-MM Date: Fri, 6 Dec 2024 03:07:00 +0000 Subject: [PATCH 29/37] lint --- daemon/src/main.rs | 4 ++-- kos_core/src/lib.rs | 1 - kos_core/src/services/krec_logger.rs | 9 +++------ platforms/stub/src/lib.rs | 7 ------- platforms/stub/src/process_manager.rs | 6 ++++++ 5 files changed, 11 insertions(+), 16 deletions(-) diff --git a/daemon/src/main.rs b/daemon/src/main.rs index b34bb22..94877a0 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -211,7 +211,7 @@ async fn main() -> Result<()> { let timestamp = Local::now().format("%Y%m%d_%H%M%S"); let temp_name = format!("kos-daemon_{}.log", timestamp); - let final_name = format!("kos-daemon_{}.log.gz", timestamp); + let final_name = format!("{}.gz", temp_name); let log_path = log_dir.join(&final_name); info!("Writing compressed logs to: {}", log_path.display()); @@ -240,7 +240,7 @@ async fn main() -> Result<()> { }; // Setup signal handler - let (shutdown_tx, mut shutdown_rx) = tokio::sync::oneshot::channel(); + let (shutdown_tx, shutdown_rx) = tokio::sync::oneshot::channel(); tokio::spawn(async move { if let Ok(()) = signal::ctrl_c().await { diff --git a/kos_core/src/lib.rs b/kos_core/src/lib.rs index a5ac1ee..3b36bb2 100644 --- a/kos_core/src/lib.rs +++ b/kos_core/src/lib.rs @@ -12,7 +12,6 @@ pub use grpc_interface::google as google_proto; pub use grpc_interface::kos as kos_proto; use async_trait::async_trait; -use eyre::Result; use hal::actuator_service_server::ActuatorServiceServer; use hal::imu_service_server::ImuServiceServer; use hal::process_manager_service_server::ProcessManagerServiceServer; diff --git a/kos_core/src/services/krec_logger.rs b/kos_core/src/services/krec_logger.rs index a4347e5..46dfc70 100644 --- a/kos_core/src/services/krec_logger.rs +++ b/kos_core/src/services/krec_logger.rs @@ -1,7 +1,4 @@ -use crate::kos_proto::{ - actuator::ActuatorStateResponse, - imu::{ImuValuesResponse, QuaternionResponse}, -}; +use crate::kos_proto::imu::{ImuValuesResponse, QuaternionResponse}; use eyre::Result; use krec::{ ActuatorCommand, ActuatorState, ImuQuaternion, ImuValues, KRec, KRecFrame, KRecHeader, Vec3, @@ -178,7 +175,7 @@ impl TelemetryLogger { tracing::error!("Failed to decode QuaternionResponse {:?}", payload); } } else if topic.contains("/actuator/state") { - match serde_json::from_slice::(&payload) { + match serde_json::from_slice::(payload) { Ok(state_list) => { for state in state_list.data { frame.actuator_states.push(ActuatorState { @@ -198,7 +195,7 @@ impl TelemetryLogger { } } } else if topic.contains("/actuator/command") { - match serde_json::from_slice::(&payload) { + match serde_json::from_slice::(payload) { Ok(command_data) => { frame.inference_step = command_data.inference_step; frame.video_timestamp = command_data.video_timestamp; diff --git a/platforms/stub/src/lib.rs b/platforms/stub/src/lib.rs index 3220fac..371f0b7 100644 --- a/platforms/stub/src/lib.rs +++ b/platforms/stub/src/lib.rs @@ -6,14 +6,7 @@ pub use imu::*; pub use process_manager::*; use async_trait::async_trait; -use eyre::Result; use kos_core::hal::Operation; -use kos_core::kos_proto::{ - actuator::actuator_service_server::ActuatorServiceServer, - imu::imu_service_server::ImuServiceServer, - process_manager::process_manager_service_server::ProcessManagerServiceServer, -}; -use kos_core::services::{ActuatorServiceImpl, IMUServiceImpl, ProcessManagerServiceImpl}; use kos_core::{services::OperationsServiceImpl, Platform, ServiceEnum}; use std::future::Future; use std::pin::Pin; diff --git a/platforms/stub/src/process_manager.rs b/platforms/stub/src/process_manager.rs index 5354966..13959cc 100644 --- a/platforms/stub/src/process_manager.rs +++ b/platforms/stub/src/process_manager.rs @@ -9,6 +9,12 @@ pub struct StubProcessManager { kclip_uuid: Mutex>, } +impl Default for StubProcessManager { + fn default() -> Self { + Self::new() + } +} + impl StubProcessManager { pub fn new() -> Self { StubProcessManager { From f04213559ed33e9cf3ad9353c53407c67149bc9f Mon Sep 17 00:00:00 2001 From: WT-MM Date: Fri, 6 Dec 2024 08:10:51 +0000 Subject: [PATCH 30/37] remove can0 --- platforms/kbot/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 7113202..22e2b8e 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -74,7 +74,7 @@ impl Platform for KbotPlatform { // "/dev/ttyCH341USB1", // "/dev/ttyCH341USB2", // "/dev/ttyCH341USB3", - "can0", "can1", "can2", + "can1", "can2", ], Duration::from_secs(1), // Duration::from_nanos(3_333_333), From 8e0e15b02056017796b7d81983eb67ac68f13744 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 10 Dec 2024 04:22:52 +0000 Subject: [PATCH 31/37] failsafes --- platforms/kbot/src/actuator.rs | 15 ++- platforms/kbot/src/lib.rs | 210 +++++++++++++++++++++++++++++---- 2 files changed, 193 insertions(+), 32 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 752d039..9180bce 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -13,6 +13,7 @@ use kos_core::{ use robstridev2::{CH341Transport, ControlConfig, SocketCanTransport, Supervisor, TransportType}; use std::time::Duration; use tokio::sync::Mutex; +use tracing::debug; pub struct KBotActuator { supervisor: Arc>, @@ -24,10 +25,10 @@ impl KBotActuator { ports: Vec<&str>, actuator_timeout: Duration, polling_interval: Duration, - desired_actuator_types: &[(u8, robstridev2::ActuatorType)], + actuators_config: &[(u8, robstridev2::ActuatorConfiguration)], ) -> Result { let mut supervisor = Supervisor::new(actuator_timeout)?; - let mut found_motors = vec![false; desired_actuator_types.len()]; + let mut found_motors = vec![false; actuators_config.len()]; for port in ports.clone() { if port.starts_with("/dev/tty") { @@ -53,23 +54,21 @@ impl KBotActuator { }); for port in ports.clone() { - let discovered_ids = supervisor - .scan_bus(0xFD, port, desired_actuator_types) - .await?; + let discovered_ids = supervisor.scan_bus(0xFD, port, actuators_config).await?; - for (idx, (motor_id, _)) in desired_actuator_types.iter().enumerate() { + for (idx, (motor_id, _)) in actuators_config.iter().enumerate() { if discovered_ids.contains(motor_id) { found_motors[idx] = true; } } } - for (idx, (motor_id, motor_type)) in desired_actuator_types.iter().enumerate() { + for (idx, (motor_id, _)) in actuators_config.iter().enumerate() { if !found_motors[idx] { tracing::warn!( "Configured motor not found - ID: {}, Type: {:?}", motor_id, - motor_type + actuators_config[idx].1.actuator_type ); } } diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 22e2b8e..cb988ea 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -5,7 +5,7 @@ mod process_manager; mod hexmove; pub use actuator::*; -pub use robstridev2::ActuatorType; +pub use robstridev2::{ActuatorConfiguration, ActuatorType}; #[cfg(target_os = "linux")] pub use hexmove::*; @@ -74,6 +74,7 @@ impl Platform for KbotPlatform { // "/dev/ttyCH341USB1", // "/dev/ttyCH341USB2", // "/dev/ttyCH341USB3", + // "can0", "can1", "can2", ], Duration::from_secs(1), @@ -81,31 +82,185 @@ impl Platform for KbotPlatform { Duration::from_millis(7), &[ // Left Arm - (11, ActuatorType::RobStride03), - (12, ActuatorType::RobStride03), - (13, ActuatorType::RobStride02), - (14, ActuatorType::RobStride02), - (15, ActuatorType::RobStride02), - (16, ActuatorType::RobStride00), + ( + 11, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 12, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 13, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 14, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 15, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 16, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride00, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), // Right Arm - (21, ActuatorType::RobStride03), - (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride02), - (24, ActuatorType::RobStride02), - (25, ActuatorType::RobStride02), - (26, ActuatorType::RobStride00), + ( + 21, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 22, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 23, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 24, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 25, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 26, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride00, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), // Left Leg - (31, ActuatorType::RobStride04), - (32, ActuatorType::RobStride03), - (33, ActuatorType::RobStride03), - (34, ActuatorType::RobStride04), - (35, ActuatorType::RobStride02), + ( + 31, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride04, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 32, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 33, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 34, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride04, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 35, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), // Right Leg - (41, ActuatorType::RobStride04), - (42, ActuatorType::RobStride03), - (43, ActuatorType::RobStride03), - (44, ActuatorType::RobStride04), - (45, ActuatorType::RobStride02), + ( + 41, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride04, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 42, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 43, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride03, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 44, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride04, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), + ( + 45, + ActuatorConfiguration { + actuator_type: ActuatorType::RobStride02, + max_angle_change: Some(4.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + ), ], ) .await @@ -125,7 +280,14 @@ impl Platform for KbotPlatform { vec!["can0"], Duration::from_secs(1), Duration::from_nanos(3_333_333), - &[(1, robstridev2::ActuatorType::RobStride04)], + &[( + 1, + robstridev2::ActuatorConfiguration { + actuator_type: ActuatorType::RobStride04, + max_angle_change: Some(2.0f32.to_radians()), + max_velocity: Some(10.0f32.to_radians()), + }, + )], ) .await .wrap_err("Failed to create actuator")?; From 2dbadc65b8da2b701d0af523f2ce5a52c8fde2b4 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 10 Dec 2024 15:16:51 -0800 Subject: [PATCH 32/37] refactored file logging --- daemon/src/file_logging.rs | 176 +++++++++++++++++++++++++++ daemon/src/main.rs | 141 ++------------------- kos_core/src/services/krec_logger.rs | 1 + 3 files changed, 187 insertions(+), 131 deletions(-) create mode 100644 daemon/src/file_logging.rs diff --git a/daemon/src/file_logging.rs b/daemon/src/file_logging.rs new file mode 100644 index 0000000..20c018c --- /dev/null +++ b/daemon/src/file_logging.rs @@ -0,0 +1,176 @@ +use chrono::Local; +use directories::BaseDirs; +use eyre::Result; +use flate2::write::GzEncoder; +use flate2::Compression; +use std::fs::File; +use std::io::{self, BufWriter, Write}; +use std::path::PathBuf; +use tracing::{info, error}; +use tracing_subscriber::prelude::*; +use tracing_subscriber::{filter::EnvFilter, Layer}; + +pub struct CompressedWriter { + encoder: Option>>, + path: PathBuf, +} + +impl CompressedWriter { + pub fn new(path: impl AsRef) -> io::Result { + let file = File::create(path.as_ref())?; + let buffered = BufWriter::new(file); + Ok(Self { + encoder: Some(GzEncoder::new(buffered, Compression::new(6))), + path: path.as_ref().to_path_buf(), + }) + } + + fn sync(&mut self) -> io::Result<()> { + if let Some(encoder) = &mut self.encoder { + encoder.flush()?; + let buf_writer = encoder.get_mut(); + buf_writer.flush()?; + let file = buf_writer.get_mut(); + file.sync_all()?; + } + Ok(()) + } + + pub fn finalize(&mut self) -> io::Result<()> { + if let Some(encoder) = self.encoder.take() { + info!("Finalizing compressed log {}", self.path.display()); + // Finish the compression and get the BufWriter back + let mut buf_writer = encoder.finish()?; + // Flush the buffer + buf_writer.flush()?; + // Sync to disk + buf_writer.get_mut().sync_all()?; + info!("Successfully finalized log {}", self.path.display()); + } + Ok(()) + } +} + +impl Write for CompressedWriter { + fn write(&mut self, buf: &[u8]) -> io::Result { + if let Some(encoder) = &mut self.encoder { + match encoder.write(buf) { + Ok(size) => { + if size > 0 && buf.contains(&b'\n') { + self.sync()?; + } + Ok(size) + } + Err(e) => { + error!( + "Failed to write to compressed log {}: {}", + self.path.display(), + e + ); + Err(e) + } + } + } else { + error!( + "Attempted to write to finalized log {}", + self.path.display() + ); + Err(io::Error::new( + io::ErrorKind::Other, + "Writer has been finalized", + )) + } + } + + fn flush(&mut self) -> io::Result<()> { + self.sync() + } +} + +impl Drop for CompressedWriter { + fn drop(&mut self) { + if let Err(e) = self.finalize() { + error!( + "Failed to finalize compressed log {}: {}", + self.path.display(), + e + ); + } + } +} + +pub fn setup_logging( + enable_file_logging: bool, + log_level: &str, +) -> Result> { + let level_filter = match log_level.to_lowercase().as_str() { + "trace" => tracing_subscriber::filter::LevelFilter::TRACE, + "debug" => tracing_subscriber::filter::LevelFilter::DEBUG, + "info" => tracing_subscriber::filter::LevelFilter::INFO, + "warn" => tracing_subscriber::filter::LevelFilter::WARN, + "error" => tracing_subscriber::filter::LevelFilter::ERROR, + _ => { + eprintln!("Invalid log level '{}', defaulting to 'info'", log_level); + tracing_subscriber::filter::LevelFilter::INFO + } + }; + + let subscriber = tracing_subscriber::registry(); + + // Update stdout layer to use the specified level + let stdout_layer = tracing_subscriber::fmt::layer() + .with_writer(std::io::stdout) + .with_filter( + EnvFilter::from_default_env() + .add_directive(format!("kos={}", log_level).parse().unwrap()) + .add_directive("h2=error".parse().unwrap()) + .add_directive("grpc=error".parse().unwrap()) + .add_directive("rumqttc=error".parse().unwrap()) + .add_directive("kos_core::telemetry=error".parse().unwrap()) + .add_directive("polling=error".parse().unwrap()) + .add_directive("async_io=error".parse().unwrap()) + .add_directive("krec=error".parse().unwrap()), + ); + + let subscriber = subscriber.with(stdout_layer); + + if enable_file_logging { + let log_dir = if let Some(base_dirs) = BaseDirs::new() { + base_dirs.data_local_dir().join("kos").join("logs") + } else { + PathBuf::from("~/.local/share/kos/logs") + }; + + std::fs::create_dir_all(&log_dir)?; + + let timestamp = Local::now().format("%Y%m%d_%H%M%S"); + let final_name = format!("kos-daemon_{}.log.gz", timestamp); + let log_path = log_dir.join(&final_name); + + let compressed_writer = CompressedWriter::new(&log_path)?; + let (non_blocking, guard) = tracing_appender::non_blocking(compressed_writer); + + let file_layer = tracing_subscriber::fmt::layer() + .with_thread_ids(true) + .with_target(true) + .with_file(true) + .with_line_number(true) + .with_writer(non_blocking) + .with_filter(level_filter); + + subscriber.with(file_layer).init(); + Ok(Some(guard)) + } else { + subscriber.init(); + Ok(None) + } +} + +pub fn cleanup_logging(guard: Option) { + if let Some(guard) = guard { + // Ensure we flush any pending writes before dropping + drop(guard); + // Give a small amount of time for the worker to finish + std::thread::sleep(std::time::Duration::from_millis(100)); + } +} diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 94877a0..814ab75 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -2,21 +2,14 @@ // This will run the gRPC server and, if applicable, a runtime loop // (e.g., actuator polling, loaded model inference). -use chrono::Local; use clap::Parser; -use directories::BaseDirs; use eyre::Result; -use flate2::write::GzEncoder; -use flate2::Compression; use kos_core::google_proto::longrunning::operations_server::OperationsServer; use kos_core::services::OperationsServiceImpl; use kos_core::telemetry::Telemetry; use kos_core::Platform; use kos_core::ServiceEnum; use std::collections::HashMap; -use std::fs::File; -use std::io::{self, BufWriter, Write}; -use std::path::PathBuf; use std::sync::Arc; use tokio::signal; use tokio::sync::Mutex; @@ -38,12 +31,19 @@ use kos_zeroth_01::Zeroth01Platform as PlatformImpl; #[cfg(feature = "kos-kbot")] use kos_kbot::KbotPlatform as PlatformImpl; +mod file_logging; +use file_logging::{setup_logging, cleanup_logging}; + #[derive(Parser, Debug)] #[command(author, version, about, long_about = None)] struct Args { /// Enable file logging #[arg(long, default_value_t = false)] log: bool, + + /// Log level (trace, debug, info, warn, error) + #[arg(long, default_value = "info")] + log_level: String, } fn add_service_to_router( @@ -87,96 +87,6 @@ struct DaemonState { platform: PlatformImpl, } -struct CompressedWriter { - encoder: Option>>, - path: PathBuf, -} - -// TODO: The encoder doesn't close properly, so this needs to be fixed later. -impl CompressedWriter { - fn new(path: impl AsRef) -> io::Result { - let file = File::create(path.as_ref())?; - let buffered = BufWriter::new(file); - Ok(Self { - encoder: Some(GzEncoder::new(buffered, Compression::new(6))), - path: path.as_ref().to_path_buf(), - }) - } - - fn sync(&mut self) -> io::Result<()> { - if let Some(encoder) = &mut self.encoder { - encoder.flush()?; - let buf_writer = encoder.get_mut(); - buf_writer.flush()?; - let file = buf_writer.get_mut(); - file.sync_all()?; - } - Ok(()) - } - - fn finalize(&mut self) -> io::Result<()> { - info!("Finalizing compressed log {}", self.path.display()); - if let Some(encoder) = self.encoder.take() { - // Finish the compression - let mut buf_writer = encoder.finish()?; - // Flush the buffer - buf_writer.flush()?; - info!("Flushed compressed log {}", self.path.display()); - // Sync to disk - buf_writer.get_mut().sync_all()?; - } - Ok(()) - } -} - -impl Write for CompressedWriter { - fn write(&mut self, buf: &[u8]) -> io::Result { - if let Some(encoder) = &mut self.encoder { - match encoder.write(buf) { - Ok(size) => { - if size > 0 && buf.contains(&b'\n') { - self.sync()?; - } - Ok(size) - } - Err(e) => { - error!( - "Failed to write to compressed log {}: {}", - self.path.display(), - e - ); - Err(e) - } - } - } else { - error!( - "Attempted to write to finalized log {}", - self.path.display() - ); - Err(io::Error::new( - io::ErrorKind::Other, - "Writer has been finalized", - )) - } - } - - fn flush(&mut self) -> io::Result<()> { - self.sync() - } -} - -impl Drop for CompressedWriter { - fn drop(&mut self) { - if let Err(e) = self.finalize() { - error!( - "Failed to finalize compressed log {}: {}", - self.path.display(), - e - ); - } - } -} - #[tokio::main] async fn main() -> Result<()> { let args = Args::parse(); @@ -198,41 +108,9 @@ async fn main() -> Result<()> { .add_directive("krec=error".parse().unwrap()), ); - let subscriber = subscriber.with(stdout_layer); + let _subscriber = subscriber.with(stdout_layer); - let guard = if args.log { - let log_dir = if let Some(base_dirs) = BaseDirs::new() { - base_dirs.data_local_dir().join("kos").join("logs") - } else { - PathBuf::from("~/.local/share/kos/logs") - }; - - std::fs::create_dir_all(&log_dir)?; - - let timestamp = Local::now().format("%Y%m%d_%H%M%S"); - let temp_name = format!("kos-daemon_{}.log", timestamp); - let final_name = format!("{}.gz", temp_name); - let log_path = log_dir.join(&final_name); - - info!("Writing compressed logs to: {}", log_path.display()); - - let compressed_writer = CompressedWriter::new(&log_path)?; - let (non_blocking, guard) = tracing_appender::non_blocking(compressed_writer); - - let file_layer = tracing_subscriber::fmt::layer() - .with_thread_ids(true) - .with_target(true) - .with_file(true) - .with_line_number(true) - .with_writer(non_blocking) - .with_filter(tracing_subscriber::filter::LevelFilter::TRACE); - - subscriber.with(file_layer).init(); - Some(guard) - } else { - subscriber.init(); - None - }; + let guard = setup_logging(args.log, &args.log_level)?; let mut state = DaemonState { _guard: guard, @@ -270,6 +148,7 @@ async fn main() -> Result<()> { } _ = shutdown_rx => { info!("Received shutdown signal, cleaning up..."); + cleanup_logging(state._guard.take()); } } diff --git a/kos_core/src/services/krec_logger.rs b/kos_core/src/services/krec_logger.rs index 46dfc70..017133c 100644 --- a/kos_core/src/services/krec_logger.rs +++ b/kos_core/src/services/krec_logger.rs @@ -39,6 +39,7 @@ struct ActuatorStateData { } #[derive(Deserialize, Debug)] +#[allow(unused)] struct ActuatorStateList { frame_number: u64, video_timestamp: u64, From ff1528e4d290e0a7b7e109ebc6c23884a48bb880 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 10 Dec 2024 16:09:39 -0800 Subject: [PATCH 33/37] Reversed disabling krec video --- platforms/kbot/src/process_manager.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/platforms/kbot/src/process_manager.rs b/platforms/kbot/src/process_manager.rs index 85e0077..15751ef 100644 --- a/platforms/kbot/src/process_manager.rs +++ b/platforms/kbot/src/process_manager.rs @@ -245,10 +245,10 @@ impl ProcessManager for KBotProcessManager { *kclip_uuid = Some(new_uuid.clone()); drop(kclip_uuid); - // let (pipeline, _sink) = Self::create_pipeline(&video_path)?; + let (pipeline, _sink) = Self::create_pipeline(&video_path)?; - // // Start the pipeline - // pipeline.set_state(gst::State::Playing)?; + // Start the pipeline + pipeline.set_state(gst::State::Playing)?; // Start telemetry logger let logger = TelemetryLogger::new( @@ -265,7 +265,7 @@ impl ProcessManager for KBotProcessManager { drop(telemetry_logger); let mut pipeline_guard = self.pipeline.lock().await; - *pipeline_guard = None; + *pipeline_guard = Some(pipeline); Ok(KClipStartResponse { clip_uuid: Some(new_uuid), From 2edf979407f3b4ef058d24eb80be770a778fd4ae Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 10 Dec 2024 16:11:11 -0800 Subject: [PATCH 34/37] robstridev2 version --- platforms/kbot/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index 15ccba1..12b6e3f 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = {version = "0.2.1", path = "../../robstridev2"} +robstridev2 = "0.3" gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" From d25cbd49cfe38923743806481c931403e0f00241 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 10 Dec 2024 16:12:12 -0800 Subject: [PATCH 35/37] unused import --- platforms/kbot/src/actuator.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 9180bce..c5857a3 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -13,7 +13,6 @@ use kos_core::{ use robstridev2::{CH341Transport, ControlConfig, SocketCanTransport, Supervisor, TransportType}; use std::time::Duration; use tokio::sync::Mutex; -use tracing::debug; pub struct KBotActuator { supervisor: Arc>, From e19f52c50790e4b91da3cefd01e219ce90cd0fb3 Mon Sep 17 00:00:00 2001 From: Wesley Maa <57124298+WT-MM@users.noreply.github.com> Date: Thu, 12 Dec 2024 02:48:34 -0800 Subject: [PATCH 36/37] Update MANIFEST.in --- MANIFEST.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MANIFEST.in b/MANIFEST.in index b3c6553..8b6054f 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1 +1 @@ -recursive-include kos_protos/ *.py *.txt py.typed MANIFEST.in py.typed Makefile +recursive-include kos_protos/ *.py *.txt py.typed MANIFEST.in py.typed Makefile pykos/Makefile From 4cd5bccb689caf98672420101ad3d51b25450c91 Mon Sep 17 00:00:00 2001 From: Wesley Maa <57124298+WT-MM@users.noreply.github.com> Date: Sat, 14 Dec 2024 11:23:13 -0800 Subject: [PATCH 37/37] Bump version to 0.1.6 --- pykos/pykos/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 44209bd..0381452 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,4 +1,4 @@ -__version__ = "0.1.5" +__version__ = "0.1.6" from pykos.client import KOS