diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index c0f000d..5f2caab 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,17 +1,17 @@ # Core components -/kos_core/ @codekansas @hatomist -/daemon/ @codekansas @hatomist +/kos_core/ @codekansas @hatomist @WT-MM +/daemon/ @codekansas @hatomist @WT-MM # Platform-specific code /platforms/kscale_micro/ @codekansas @hatomist /platforms/kscale_pro/ @codekansas @hatomist @WT-MM -/platforms/sim/ @codekansas @hatomist +/platforms/sim/ @codekansas @hatomist @WT-MM /platforms/stub/ @codekansas @hatomist # Python client -/pykos/ @codekansas @hatomist +/pykos/ @codekansas @hatomist @WT-MM # Build and CI configuration -/.github/ @codekansas @hatomist -/toolchain/ @codekansas @hatomist -*.toml @codekansas @hatomist \ No newline at end of file +/.github/ @codekansas @hatomist @WT-MM +/toolchain/ @codekansas @hatomist @WT-MM +*.toml @codekansas @hatomist @WT-MM diff --git a/daemon/src/main.rs b/daemon/src/main.rs index bc73893..93476a8 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -42,10 +42,10 @@ async fn run_server( platform: &(dyn Platform + Send + Sync), operations_service: Arc, ) -> Result<(), Box> { - let addr = "[::1]:50051".parse()?; + 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())?; let operations_service = OperationsServer::new(operations_service); diff --git a/kos_core/src/hal.rs b/kos_core/src/hal.rs index c333021..00f48c4 100644 --- a/kos_core/src/hal.rs +++ b/kos_core/src/hal.rs @@ -5,6 +5,7 @@ pub use crate::kos_proto::{actuator::*, common::ActionResult, imu::*}; use async_trait::async_trait; use eyre::Result; use std::fmt::Display; + #[async_trait] pub trait Actuator: Send + Sync { async fn command_actuators(&self, commands: Vec) -> Result>; diff --git a/kos_core/src/lib.rs b/kos_core/src/lib.rs index 609979d..4c4b0ee 100644 --- a/kos_core/src/lib.rs +++ b/kos_core/src/lib.rs @@ -40,7 +40,10 @@ pub trait Platform { fn name(&self) -> &'static str; fn serial(&self) -> String; fn initialize(&mut self, operations_service: Arc) -> eyre::Result<()>; - fn create_services(&self, operations_service: Arc) -> Vec; + fn create_services( + &self, + operations_service: Arc, + ) -> eyre::Result>; fn shutdown(&mut self) -> eyre::Result<()>; } diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index 1034b0d..0bcf292 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -10,5 +10,7 @@ description = "KOS platform for KBot" [dependencies] kos_core = { version = "0.1.1", path = "../../kos_core" } eyre = "0.6" +tracing = "0.1" +async-trait = "0.1" robstride = "0.2.8" imu = "0.1.4" \ No newline at end of file diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 7b73ccd..7ed66a2 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -1,64 +1,151 @@ +use crate::{Arc, Operation, OperationsServiceImpl}; use async_trait::async_trait; -use eyre::Result; -use kos_core::google_proto::longrunning::Operation; -use kos_core::services::OperationsServiceImpl; +use eyre::{Result, WrapErr}; use kos_core::{ hal::{ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorRequest}, - kos_proto::{actuator::*, common::ActionResult}, + kos_proto::{ + actuator::*, + common::Error as KosError, + common::{ActionResult, ErrorCode}, + }, }; use std::collections::HashMap; -use std::sync::Arc; -use robstride::{MotorMode, MotorType, Motors, MotorsSupervisor}; +use robstride::{MotorType, MotorsSupervisor}; -pub struct KscaleProActuator { +pub struct KBotActuator { motors_supervisor: MotorsSupervisor, } -impl KscaleProActuator { +impl KBotActuator { pub fn new( + _operations_service: Arc, port: &str, motor_infos: HashMap, verbose: Option, max_update_rate: Option, zero_on_init: Option, - ) -> Self { + ) -> 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, + &motor_infos_u8, verbose.unwrap_or(false), - max_update_rate.unwrap_or(100000), + max_update_rate.unwrap_or(100000) as f64, zero_on_init.unwrap_or(false), ) - .unwrap(); + .map_err(|e| eyre::eyre!("Failed to create motors supervisor: {}", e))?; - KscaleProActuator { - motors_supervisor, - } + Ok(KBotActuator { motors_supervisor }) } } #[async_trait] -impl Actuator for KscaleProActuator { - async fn command_actuators( - &self, - _commands: Vec, - ) -> Result> { - Ok(vec![]) +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 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 + }; + + results.push(ActionResult { + actuator_id: command.actuator_id, + success, + error, + }); + } + Ok(results) } - async fn configure_actuator( - &self, - _config: ConfigureActuatorRequest, - ) -> Result { - Ok(ActionResponse { - success: true, - error: None, - }) + 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); + } + + // 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); + } + + 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 + }; + + Ok(ActionResponse { success, error }) } - async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result { + async fn calibrate_actuator(&self, _request: CalibrateActuatorRequest) -> Result { Ok(Operation::default()) } @@ -70,11 +157,11 @@ impl Actuator for KscaleProActuator { Ok(feedback .iter() .map(|(id, state)| ActuatorStateResponse { - actuator_id: *id, - online: state.mode==MotorMode.Motor, - position: state.position, - velocity: state.velocity, - torque: state.torque, + 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, diff --git a/platforms/kbot/src/hexmove.rs b/platforms/kbot/src/hexmove.rs index 8fa7b6e..9882d7c 100644 --- a/platforms/kbot/src/hexmove.rs +++ b/platforms/kbot/src/hexmove.rs @@ -1,45 +1,83 @@ - use kos_core::{ - hal::{ - CalibrateImuMetadata, CalibrationStatus, EulerAnglesResponse, ImuValuesResponse, - QuaternionResponse, IMU, Operation, - }, + hal::{EulerAnglesResponse, ImuValuesResponse, Operation, QuaternionResponse, IMU}, kos_proto::common::ActionResponse, + services::OperationsServiceImpl, }; +use async_trait::async_trait; +use eyre::{Result, WrapErr}; use imu::hexmove::*; +use std::sync::Arc; +use std::time::Duration; +use tracing::{debug, error, info, trace}; -pub struct KscaleProIMU { - operations_service: Arc, +pub struct KBotIMU { + _operations_service: Arc, imu: ImuReader, } -impl KscaleProIMU { - pub fn new(interface: &str, can_id: u32, model: u32) -> Self { - KscaleProIMU { - operations_service, - imu: ImuReader::new(interface, can_id, model).unwrap(), - } +impl KBotIMU { + pub fn new( + operations_service: Arc, + interface: &str, + can_id: u32, + model: u32, + ) -> Result { + info!( + "Initializing KBotIMU with interface: {}, CAN ID: {}, model: {}", + interface, can_id, model + ); + + let can_id = + u8::try_from(can_id).wrap_err_with(|| format!("CAN ID {} too large for u8", can_id))?; + let model = + u8::try_from(model).wrap_err_with(|| format!("Model ID {} too large for u8", model))?; + + let imu = match ImuReader::new(interface, can_id, model) { + Ok(imu) => { + info!("Successfully created IMU reader"); + imu + } + Err(e) => { + error!("Failed to create IMU reader: {}", e); + return Err(eyre::eyre!("Failed to create IMU reader: {}", e)); + } + }; + + Ok(KBotIMU { + _operations_service: operations_service, + imu, + }) } } -impl Default for KscaleProIMU { +impl Default for KBotIMU { fn default() -> Self { - unimplemented!("KscaleProIMU cannot be default, it requires an operations store") + unimplemented!("KBotIMU cannot be default, it requires an operations store") } } #[async_trait] -impl IMU for StubIMU { +impl IMU for KBotIMU { async fn get_values(&self) -> Result { let data = self.imu.get_data(); + trace!( + "Reading IMU values, accel x: {}, y: {}, z: {}, angle x: {}, y: {}, z: {}", + data.x_velocity, + data.y_velocity, + data.z_velocity, + data.x_angle, + data.y_angle, + data.z_angle + ); + Ok(ImuValuesResponse { - accel_x: None, - accel_y: None, - accel_z: None, - gyro_x: None, - gyro_y: None, - gyro_z: None, + accel_x: data.x_velocity as f64, + accel_y: data.y_velocity as f64, + accel_z: data.z_velocity as f64, + gyro_x: 0 as f64, + gyro_y: 0 as f64, + gyro_z: 0 as f64, mag_x: None, mag_y: None, mag_z: None, @@ -48,10 +86,17 @@ impl IMU for StubIMU { } async fn calibrate(&self) -> Result { - Ok(Operation::default()) + info!("Starting IMU calibration - unimplemented"); + Ok(Operation { + name: "operations/calibrate_imu/0".to_string(), + metadata: None, + done: true, + result: None, + }) } - async fn zero(&self, _duration: Duration) -> Result { + async fn zero(&self, duration: Duration) -> Result { + info!("Zeroing IMU with duration: {:?} - unimplemented", duration); Ok(ActionResponse { success: true, error: None, @@ -59,22 +104,18 @@ impl IMU for StubIMU { } async fn get_euler(&self) -> Result { + debug!("Reading Euler angles"); let data = self.imu.get_data(); Ok(EulerAnglesResponse { - roll: data.x_angle, - pitch: data.y_angle, - yaw: data.z_angle, + roll: data.x_angle as f64, + pitch: data.y_angle as f64, + yaw: data.z_angle as f64, error: None, }) } async fn get_quaternion(&self) -> Result { - Ok(QuaternionResponse { - w: 1.0, - x: 0.0, - y: 0.0, - z: 0.0, - error: None, - }) + error!("Quaternion operation not implemented"); + Err(eyre::eyre!("Not implemented")) } } diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index d2e0190..d32ed3c 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -4,6 +4,7 @@ mod hexmove; pub use actuator::*; pub use hexmove::*; +use eyre::{Result, WrapErr}; use kos_core::hal::Operation; use kos_core::kos_proto::{ actuator::actuator_service_server::ActuatorServiceServer, @@ -11,6 +12,8 @@ use kos_core::kos_proto::{ }; use kos_core::services::{ActuatorServiceImpl, IMUServiceImpl}; use kos_core::{services::OperationsServiceImpl, Platform, ServiceEnum}; +use robstride::MotorType; +use std::collections::HashMap; use std::sync::Arc; pub struct KbotPlatform {} @@ -41,19 +44,36 @@ impl Platform for KbotPlatform { Ok(()) } - fn create_services(&self, operations_service: Arc) -> Vec { - // Add available services here - vec![ + fn create_services( + &self, + operations_service: Arc, + ) -> Result> { + Ok(vec![ ServiceEnum::Imu(ImuServiceServer::new(IMUServiceImpl::new(Arc::new( - KbotIMU::new("can0", 1, 1), + KBotIMU::new(operations_service.clone(), "can0", 1, 1) + .wrap_err("Failed to create IMU")?, )))), ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( - Arc::new(KbotActuator::new( - "/dev/ttyCH341USB0", - HashMap::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")?, + ), ))), - ] + ]) } fn shutdown(&mut self) -> eyre::Result<()> { diff --git a/platforms/stub/src/lib.rs b/platforms/stub/src/lib.rs index 5a3bccd..13a0308 100644 --- a/platforms/stub/src/lib.rs +++ b/platforms/stub/src/lib.rs @@ -4,6 +4,7 @@ mod imu; pub use actuator::*; pub use imu::*; +use eyre::Result; use kos_core::hal::Operation; use kos_core::kos_proto::{ actuator::actuator_service_server::ActuatorServiceServer, @@ -41,16 +42,19 @@ impl Platform for StubPlatform { Ok(()) } - fn create_services(&self, operations_service: Arc) -> Vec { + fn create_services( + &self, + operations_service: Arc, + ) -> Result> { // Add available services here - vec![ + 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())), ))), - ] + ]) } fn shutdown(&mut self) -> eyre::Result<()> {