diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 6c3c3a3..50046bf 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -27,10 +27,13 @@ jobs: - name: Checkout code uses: actions/checkout@v3 + - name: Change directory + run: cd pykos + - name: Set up Python uses: actions/setup-python@v4 with: - python-version: "3.11" + python-version: "3.8" - name: Install dependencies run: | diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 04aa24d..e56596c 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -25,10 +25,13 @@ jobs: - name: Check out repository uses: actions/checkout@v3 + - name: Change directory + run: cd pykos + - name: Set up Python uses: actions/setup-python@v4 with: - python-version: "3.11" + python-version: "3.8" - name: Restore cache id: restore-cache diff --git a/.gitignore b/.gitignore index cfe6730..239dfb6 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,11 @@ __pycache__/ .pytest_cache/ .ruff_cache/ .dmypy.json +venv/ + +# python protobuf +pykos/kos/ +!pykos/kos/__init__.py # Rust target/ @@ -18,6 +23,9 @@ Cargo.lock # Databases *.db +# Gitlab +.gitlab-ci-local/ + # Build artifacts build/ dist/ diff --git a/.gitlab-ci-local-env b/.gitlab-ci-local-env new file mode 100644 index 0000000..120a4e4 --- /dev/null +++ b/.gitlab-ci-local-env @@ -0,0 +1,6 @@ +PRIVILEGED=true +ULIMIT=8000:16000 +VOLUME=certs:/certs/client +VOLUME="/var/run/docker.sock:/var/run/docker.sock" +VARIABLE="DOCKER_TLS_CERTDIR=/certs" +NEEDS=true diff --git a/.gitlab-ci-local-variables.yml b/.gitlab-ci-local-variables.yml new file mode 100644 index 0000000..cffa814 --- /dev/null +++ b/.gitlab-ci-local-variables.yml @@ -0,0 +1,2 @@ +TOOLCHAIN_IMAGE: "openlch-runtime-sdk" +LOCAL_BUILD: true \ No newline at end of file diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..49b8824 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,75 @@ +image: kos-builder:latest + +variables: + CARGO_HOME: $CI_PROJECT_DIR/.cargo + CARGO_TERM_COLOR: always + RUST_BACKTRACE: 1 + +# Cache dependencies between builds +cache: + paths: + - .cargo + - target/ + +# Define stages +stages: + - check + - test + - build + +# Check formatting and run clippy on all branches +format: + stage: check + script: + - cargo fmt -- --check + rules: + - when: always + +clippy: + stage: check + script: + - cargo clippy -- -D warnings + rules: + - when: always + +# Test all features +test: + stage: test + script: + - | + if [ "$GITLAB_CI" != "false" ]; then + # CI-specific test commands + cargo test --all-features --verbose + else + # Local test commands + cargo test + fi + rules: + - when: always + +# Build binaries only on tags/releases +.build_template: &build_definition + stage: build + variables: + CROSS_REMOTE: 1 + script: + - cross build --release --target $TARGET --features $FEATURES --no-default-features + artifacts: + paths: + - target/$TARGET/release/daemon + rules: + - if: $CI_COMMIT_TAG + +# Linux x86_64 +build-linux-x86_64-stub-release: + <<: *build_definition + variables: + TARGET: x86_64-unknown-linux-gnu + FEATURES: stub + +# Linux aarch64 +build-linux-aarch64-stub-release: + <<: *build_definition + variables: + TARGET: aarch64-unknown-linux-gnu + FEATURES: stub \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..c59ad0d --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "protos/googleapis"] + path = proto/googleapis + url = https://github.com/googleapis/googleapis.git diff --git a/Cargo.toml b/Cargo.toml index ea029b1..56b35a4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,18 +1,27 @@ [workspace] +resolver = "2" members = [ - "kos/bindings", - "kos/kos", + "kos_core", + "daemon", ] -resolver = "2" -[workspace.package] +exclude = [ + "platforms", +] -version = "0.1.0" -authors = ["Wesley Maa ", "Pawel Budzianowski ", "Benjamin Bolte "] +[workspace.package] +version = "0.1.1" +authors = [ + "Benjamin Bolte ", + "Denys Bezmenov ", + "Jingxiang Mo ", + "Pawel Budzianowski ", + "Wesley Maa ", +] edition = "2021" license = "MIT" repository = "https://github.com/kscalelabs/kos" description = "The K-Scale Operating System" -documentation = "https://docs.kscale.dev/kos/intro" +documentation = "https://docs.kscale.dev/os/intro" readme = "README.md" diff --git a/Cross.toml b/Cross.toml new file mode 100644 index 0000000..a13245a --- /dev/null +++ b/Cross.toml @@ -0,0 +1,6 @@ +[target.aarch64-unknown-linux-gnu] +image = "ubuntu:24.04" +pre-build = [ + "apt-get update", + "apt-get install -y protobuf-compiler gcc-aarch64-linux-gnu g++-aarch64-linux-gnu build-essential" +] \ No newline at end of file diff --git a/daemon/Cargo.toml b/daemon/Cargo.toml new file mode 100644 index 0000000..eba39e9 --- /dev/null +++ b/daemon/Cargo.toml @@ -0,0 +1,30 @@ +[package] +name = "daemon" +version = "0.1.0" +edition = "2021" + +[dependencies] +kos_core = { path = "../kos_core" } +kscale_micro = { path = "../platforms/kscale_micro", optional = true } +tokio = { version = "1", features = ["full"] } +tonic = { version = "0.12", features = ["transport"] } +eyre = "0.6" +tracing = "0.1" +tracing-subscriber = { version = "0.3", features = ["env-filter"] } +tower = "0.5" + +sim = { path = "../platforms/sim", optional = true } +stub = { path = "../platforms/stub", optional = true } + +[target.'cfg(target_os = "linux")'.dependencies] +kscale_pro = { path = "../platforms/kscale_pro", optional = true } + +# Also can add external platforms here? +# e.g. third_party_platform = { git = "https://github.com/thirdparty/platform", optional = true } + +[features] +kscale_micro = ["dep:kscale_micro"] +kscale_pro = ["dep:kscale_pro"] +sim = ["dep:sim"] +stub = ["dep:stub"] +default = ["stub"] diff --git a/daemon/src/main.rs b/daemon/src/main.rs new file mode 100644 index 0000000..6bec3a6 --- /dev/null +++ b/daemon/src/main.rs @@ -0,0 +1,88 @@ +// TODO: Implement daemon for managing the robot. +// This will run the gRPC server and, if applicable, a runtime loop +// (e.g., actuator polling, loaded model inference). + +use eyre::Result; +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::sync::Arc; +use tokio::sync::Mutex; +use tonic::transport::Server; +use tracing::{debug, error, info}; +use tracing_subscriber::filter::EnvFilter; + +#[cfg(feature = "sim")] +use sim::SimPlatform as PlatformImpl; + +#[cfg(feature = "stub")] +use stub::StubPlatform as PlatformImpl; + +fn add_service_to_router( + router: tonic::transport::server::Router, + service: ServiceEnum, +) -> tonic::transport::server::Router { + debug!("Adding service to router: {:?}", service); + match service { + ServiceEnum::Actuator(svc) => router.add_service(svc), + ServiceEnum::Imu(svc) => router.add_service(svc), + } +} + +async fn run_server( + platform: &(dyn Platform + Send + Sync), + operations_service: Arc, +) -> Result<(), Box> { + let addr = "[::1]:50051".parse()?; + let mut server_builder = Server::builder(); + + let services = platform.create_services(operations_service.clone()); + + let operations_service = OperationsServer::new(operations_service); + + let mut router = server_builder.add_service(operations_service); + + // Add remaining services using the helper function + for service in services { + router = add_service_to_router(router, service); + } + + info!("Serving on {}", addr); + // Serve the accumulated router + router.serve(addr).await?; + Ok(()) +} + +#[tokio::main] +async fn main() -> Result<()> { + // logging + tracing_subscriber::fmt() + .with_env_filter( + EnvFilter::from_default_env() + .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()), + ) + .init(); + + // telemetry + Telemetry::initialize("test", "localhost", 1883).await?; + + let operations_store = Arc::new(Mutex::new(HashMap::new())); + let operations_service = Arc::new(OperationsServiceImpl::new(operations_store)); + + let mut platform = PlatformImpl::new(); + + platform.initialize(operations_service.clone())?; + + if let Err(e) = run_server(&platform, operations_service).await { + error!("Server error: {:?}", e); + std::process::exit(1); + } + + Ok(()) +} diff --git a/kos/__init__.py b/kos/__init__.py deleted file mode 100644 index f102a9c..0000000 --- a/kos/__init__.py +++ /dev/null @@ -1 +0,0 @@ -__version__ = "0.0.1" diff --git a/kos/bindings/Cargo.toml b/kos/bindings/Cargo.toml deleted file mode 100644 index c90db1e..0000000 --- a/kos/bindings/Cargo.toml +++ /dev/null @@ -1,33 +0,0 @@ -[package] - -name = "bindings" -version.workspace = true -description.workspace = true -readme.workspace = true -authors.workspace = true -edition.workspace = true -repository.workspace = true -license.workspace = true - -[lib] - -name = "bindings" -crate-type = ["cdylib", "rlib"] - -[dependencies] - -pyo3 = { version = ">= 0.21.0", features = ["extension-module"] } -pyo3-stub-gen = ">= 0.6.0" - -# Other packages in the workspace. -# kos = { path = "../kos" } - -[[bin]] - -name = "hello_world" -path = "src/bin/hello_world.rs" - -[[bin]] - -name = "stub_gen" -path = "src/bin/stub_gen.rs" diff --git a/kos/bindings/bindings.pyi b/kos/bindings/bindings.pyi deleted file mode 100644 index c0084f5..0000000 --- a/kos/bindings/bindings.pyi +++ /dev/null @@ -1,7 +0,0 @@ -# This file is automatically generated by pyo3_stub_gen -# ruff: noqa: E501, F401 - - -def hello_world() -> None: - ... - diff --git a/kos/bindings/pyproject.toml b/kos/bindings/pyproject.toml deleted file mode 100644 index 4117fe1..0000000 --- a/kos/bindings/pyproject.toml +++ /dev/null @@ -1,10 +0,0 @@ -[build-system] -requires = ["maturin>=1.1,<2.0"] -build-backend = "maturin" - -[project] -name = "bindings" -requires-python = ">=3.9" - -[project.optional-dependencies] -test = ["pytest", "pyright", "ruff"] diff --git a/kos/bindings/src/bin/hello_world.rs b/kos/bindings/src/bin/hello_world.rs deleted file mode 100644 index 4382745..0000000 --- a/kos/bindings/src/bin/hello_world.rs +++ /dev/null @@ -1,5 +0,0 @@ -use bindings::hello_world; - -fn main() { - hello_world(); -} diff --git a/kos/bindings/src/bin/stub_gen.rs b/kos/bindings/src/bin/stub_gen.rs deleted file mode 100644 index ceb1044..0000000 --- a/kos/bindings/src/bin/stub_gen.rs +++ /dev/null @@ -1,7 +0,0 @@ -use pyo3_stub_gen::Result; - -fn main() -> Result<()> { - let stub = bindings::stub_info()?; - stub.generate()?; - Ok(()) -} diff --git a/kos/bindings/src/lib.rs b/kos/bindings/src/lib.rs deleted file mode 100644 index 36355bf..0000000 --- a/kos/bindings/src/lib.rs +++ /dev/null @@ -1,21 +0,0 @@ -// use kscaleos::hello_world as kscaleos_hello_world; -use pyo3::prelude::*; -use pyo3::{wrap_pyfunction, PyResult}; -use pyo3_stub_gen::define_stub_info_gatherer; -use pyo3_stub_gen::derive::gen_stub_pyfunction; - -#[gen_stub_pyfunction] -#[pyfunction] -pub fn hello_world() -> PyResult<()> { - // kscaleos_hello_world(); - println!("Hello, world!"); - Ok(()) -} - -#[pymodule] -fn bindings(m: &Bound) -> PyResult<()> { - m.add_function(wrap_pyfunction!(hello_world, m)?).unwrap(); - Ok(()) -} - -define_stub_info_gatherer!(stub_info); diff --git a/kos/kos/Cargo.toml b/kos/kos/Cargo.toml deleted file mode 100644 index e4e4902..0000000 --- a/kos/kos/Cargo.toml +++ /dev/null @@ -1,35 +0,0 @@ -[package] - -name = "kos" -version.workspace = true -description.workspace = true -readme.workspace = true -authors.workspace = true -edition.workspace = true -repository.workspace = true -license.workspace = true - -[lib] - -name = "kos" -crate-type = ["cdylib", "rlib"] - -[[bin]] - -name = "kos" -path = "src/bin.rs" - -[dependencies] -robstride = "0.2.1" -tonic = "0.12.3" -tonic-web = "0.12.3" - -# Serde is used for parsing configuration files. -serde = { version = "1.0", features = ["derive"] } -serde_yaml = "0.9" - -[target.'cfg(any(target_os = "macos", target_os = "linux"))'.dependencies] -ort = { version = "1.16.3", optional = true } - -[build-dependencies] -tonic-build = "0.12.3" diff --git a/kos/kos/src/bin/run.rs b/kos/kos/src/bin/run.rs deleted file mode 100644 index 94c62b8..0000000 --- a/kos/kos/src/bin/run.rs +++ /dev/null @@ -1,5 +0,0 @@ -use kos::main as kos_main; - -fn main() { - kos_main(); -} diff --git a/kos/kos/src/config.rs b/kos/kos/src/config.rs deleted file mode 100644 index d957d6c..0000000 --- a/kos/kos/src/config.rs +++ /dev/null @@ -1,29 +0,0 @@ -use robstride::MotorType; -use serde::{Deserialize, Serialize}; -use std::collections::HashMap; - -#[derive(Serialize, Deserialize, Debug)] -pub struct MotorConfig { - pub motor_type: MotorType, - pub kp: f32, - pub kd: f32, -} - -#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Hash)] -pub enum Limb { - LeftArm, - RightArm, - LeftLeg, - RightLeg, -} - -#[derive(Serialize, Deserialize, Debug)] -pub struct LimbConfig { - pub motor_configs: HashMap, - pub port_name: String, -} - -#[derive(Serialize, Deserialize, Debug)] -pub struct Config { - pub limbs: HashMap, -} diff --git a/kos/kos/src/lib.rs b/kos/kos/src/lib.rs deleted file mode 100644 index 28b659f..0000000 --- a/kos/kos/src/lib.rs +++ /dev/null @@ -1,10 +0,0 @@ -mod config; -mod runner; -mod state; - -use runner::run; -use std::sync::{Arc, RwLock}; - -pub fn main() { - run(Arc::new(RwLock::new(state::State::new()))); -} diff --git a/kos/kos/src/runner.rs b/kos/kos/src/runner.rs deleted file mode 100644 index c413f41..0000000 --- a/kos/kos/src/runner.rs +++ /dev/null @@ -1,17 +0,0 @@ -/** - * Defines the main control loop that runs in its own independent thread. - */ -use std::{ - sync::{Arc, RwLock}, - thread, - time::Duration, -}; - -use crate::state::State; - -pub fn run(_state: Arc>) { - loop { - println!("Running main control loop..."); - thread::sleep(Duration::from_millis(100)); - } -} diff --git a/kos/kos/src/state.rs b/kos/kos/src/state.rs deleted file mode 100644 index e002c17..0000000 --- a/kos/kos/src/state.rs +++ /dev/null @@ -1,13 +0,0 @@ -/** - * Defines state variables that are passed between the runner loop and - * the calling process. - */ - -#[derive(Debug)] -pub struct State {} - -impl State { - pub fn new() -> Self { - State {} - } -} diff --git a/kos/requirements-dev.txt b/kos/requirements-dev.txt deleted file mode 100644 index 0cf2833..0000000 --- a/kos/requirements-dev.txt +++ /dev/null @@ -1,10 +0,0 @@ -# requirements-dev.txt - -# Linting -black -darglint -mypy -ruff - -# Testing -pytest diff --git a/kos/requirements.txt b/kos/requirements.txt deleted file mode 100644 index 2ee1e7b..0000000 --- a/kos/requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -# requirements.txt - diff --git a/kos_core/Cargo.toml b/kos_core/Cargo.toml new file mode 100644 index 0000000..50b66ca --- /dev/null +++ b/kos_core/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "kos_core" +version = "0.1.0" +edition = "2021" +build = "build.rs" + +[dependencies] +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" +yaml-rust2 = "0.9" +tonic = { version = "0.12", features = ["transport"] } +prost = "0.13" +prost-types = "0.13" +async-trait = "0.1" +rumqttc = "0.24" +tokio = { version = "1", features = ["full"] } +eyre = "0.6" +hyper = "0.14" +tracing = "0.1" +lazy_static = "1.4" + +[build-dependencies] +tonic-build = "0.12" + +[lib] +doctest = false diff --git a/kos_core/build.rs b/kos_core/build.rs new file mode 100644 index 0000000..8eabab9 --- /dev/null +++ b/kos_core/build.rs @@ -0,0 +1,40 @@ +use std::env; +use std::path::PathBuf; + +fn main() { + // Path to the Protobuf files + let proto_root = "../proto"; + + // Where to output the compiled Rust files + let out_dir = PathBuf::from(env::var("OUT_DIR").unwrap()); + + // List of Protobuf files + let protos = [ + "kos/common.proto", + "kos/actuator.proto", + "kos/imu.proto", + "kos/inference.proto", + "kos/process_manager.proto", + "kos/system.proto", + "google/longrunning/operations.proto", + ]; + + let includes = [proto_root, &format!("{}/googleapis", proto_root)]; + + // Create the output directory + std::fs::create_dir_all(out_dir.join("kos")).expect("Failed to create output directory"); + + // Configure and compile Protobuf files + tonic_build::configure() + .build_server(true) + .out_dir(out_dir.join("kos")) + .protoc_arg("--experimental_allow_proto3_optional") + .compile_protos(&protos, &includes) + .expect("Failed to compile protos"); + + // Re-run the build script if any of the proto files change + for proto in &protos { + println!("cargo:rerun-if-changed={}/kos/{}", proto_root, proto); + } + println!("cargo:rerun-if-changed={}", proto_root); +} diff --git a/kos_core/src/config.rs b/kos_core/src/config.rs new file mode 100644 index 0000000..68a0c63 --- /dev/null +++ b/kos_core/src/config.rs @@ -0,0 +1,3 @@ +// TODO: Implement config loading. +// Config should include embodiment information (e.g. limb names, actuator names), +// as well as hardware parameters (e.g. serial port names, motor types, PID gains). diff --git a/kos_core/src/grpc_interface.rs b/kos_core/src/grpc_interface.rs new file mode 100644 index 0000000..2d854a2 --- /dev/null +++ b/kos_core/src/grpc_interface.rs @@ -0,0 +1,39 @@ +pub mod kos { + pub mod actuator { + tonic::include_proto!("kos/kos.actuator"); + } + + pub mod common { + tonic::include_proto!("kos/kos.common"); + } + + pub mod imu { + tonic::include_proto!("kos/kos.imu"); + } + + pub mod inference { + tonic::include_proto!("kos/kos.inference"); + } + + pub mod process_manager { + tonic::include_proto!("kos/kos.processmanager"); + } + + pub mod system { + tonic::include_proto!("kos/kos.system"); + } +} + +pub mod google { + pub mod longrunning { + tonic::include_proto!("kos/google.longrunning"); + } + + pub mod api { + tonic::include_proto!("kos/google.api"); + } + + pub mod rpc { + tonic::include_proto!("kos/google.rpc"); + } +} diff --git a/kos_core/src/hal.rs b/kos_core/src/hal.rs new file mode 100644 index 0000000..c333021 --- /dev/null +++ b/kos_core/src/hal.rs @@ -0,0 +1,43 @@ +pub use crate::grpc_interface::google::longrunning::*; +pub use crate::grpc_interface::kos; +pub use crate::grpc_interface::kos::common::ActionResponse; +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>; + async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result; + async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result; + async fn get_actuators_state( + &self, + actuator_ids: Vec, + ) -> Result>; +} + +#[async_trait] +pub trait IMU: Send + Sync { + async fn get_values(&self) -> Result; + async fn calibrate(&self) -> Result; + async fn zero(&self, duration: std::time::Duration) -> Result; + async fn get_euler(&self) -> Result; + async fn get_quaternion(&self) -> Result; +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum CalibrationStatus { + Calibrating, + Calibrated, + Timeout, +} + +impl Display for CalibrationStatus { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + CalibrationStatus::Calibrating => write!(f, "calibrating"), + CalibrationStatus::Calibrated => write!(f, "calibrated"), + CalibrationStatus::Timeout => write!(f, "timeout"), + } + } +} diff --git a/kos_core/src/lib.rs b/kos_core/src/lib.rs new file mode 100644 index 0000000..f579a02 --- /dev/null +++ b/kos_core/src/lib.rs @@ -0,0 +1,65 @@ +#![allow(unknown_lints)] +#![allow(clippy::doc_lazy_continuation)] + +pub mod config; +mod grpc_interface; +pub mod hal; +pub mod process_manager; +pub mod services; +pub mod telemetry; +pub mod telemetry_types; + +pub use grpc_interface::google as google_proto; +pub use grpc_interface::kos as kos_proto; + +use hal::actuator_service_server::ActuatorServiceServer; +use hal::imu_service_server::ImuServiceServer; +use services::OperationsServiceImpl; +use services::{ActuatorServiceImpl, IMUServiceImpl}; +use std::fmt::Debug; +use std::sync::Arc; + +impl Debug for ActuatorServiceImpl { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "ActuatorServiceImpl") + } +} +impl Debug for IMUServiceImpl { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "IMUServiceImpl") + } +} + +#[derive(Debug)] +pub enum ServiceEnum { + Actuator(ActuatorServiceServer), + Imu(ImuServiceServer), +} + +pub trait Platform { + fn name(&self) -> &'static str; + fn initialize(&mut self, operations_service: Arc) -> eyre::Result<()>; + fn create_services(&self, operations_service: Arc) -> Vec; + fn shutdown(&mut self) -> eyre::Result<()>; +} + +#[cfg(test)] +mod tests { + // use super::*; + + // fn test_config_loading() { + // let yaml = r#" + // limbs: + // LeftArm: + // port_name: /dev/ttyUSB0 + // motor_configs: + // 1: + // motor_type: Type01 + // kp: 50.0 + // kd: 1.0 + // "#; + // let config: Config = serde_yaml::from_str(yaml).expect("Failed to parse YAML"); + // assert_eq!(config.limbs.len(), 1); + // assert_eq!(config.limbs.contains_key("LeftArm"), true); + // } +} diff --git a/kos_core/src/process_manager.rs b/kos_core/src/process_manager.rs new file mode 100644 index 0000000..07f38ff --- /dev/null +++ b/kos_core/src/process_manager.rs @@ -0,0 +1,2 @@ +// TODO: Implement process manager. +// This will manage life cycle of non rust services (e.g. gstreamer, mosquitto etc) diff --git a/kos_core/src/services/actuator.rs b/kos_core/src/services/actuator.rs new file mode 100644 index 0000000..1380a4c --- /dev/null +++ b/kos_core/src/services/actuator.rs @@ -0,0 +1,111 @@ +use crate::grpc_interface::google::longrunning::Operation; +use crate::hal::Actuator; +use crate::kos_proto::actuator::actuator_service_server::ActuatorService; +use crate::kos_proto::actuator::*; +use crate::kos_proto::common::ActionResponse; +use crate::telemetry::Telemetry; +use crate::telemetry_types::{ActuatorCommand, ActuatorState}; +use std::sync::Arc; +use tonic::{Request, Response, Status}; +use tracing::trace; + +pub struct ActuatorServiceImpl { + actuator: Arc, +} + +impl ActuatorServiceImpl { + pub fn new(actuator: Arc) -> Self { + Self { actuator } + } +} + +#[tonic::async_trait] +impl ActuatorService for ActuatorServiceImpl { + async fn command_actuators( + &self, + request: Request, + ) -> Result, Status> { + let commands = request.into_inner().commands; + + let telemetry_commands: Vec<_> = commands.iter().map(ActuatorCommand::from).collect(); + + let results = self + .actuator + .command_actuators(commands) + .await + .map_err(|e| Status::internal(format!("Failed to command actuators, {:?}", e)))?; + + trace!( + "Commanding actuators, request: {:?}, results: {:?}", + telemetry_commands.clone(), + results + ); + + let telemetry = Telemetry::get().await; + if let Some(telemetry) = telemetry { + if let Err(e) = telemetry + .publish("actuator/command", &telemetry_commands) + .await + { + tracing::warn!("Failed to publish telemetry: {}", e); + } + } + + Ok(Response::new(CommandActuatorsResponse { results })) + } + + async fn configure_actuator( + &self, + request: Request, + ) -> Result, Status> { + let config = request.into_inner(); + let response = self + .actuator + .configure_actuator(config) + .await + .map_err(|e| Status::internal(format!("Failed to configure actuator, {:?}", e)))?; + Ok(Response::new(response)) + } + + async fn calibrate_actuator( + &self, + request: Request, + ) -> Result, Status> { + let calibrate_request = request.into_inner(); + let operation = self + .actuator + .calibrate_actuator(calibrate_request) + .await + .map_err(|e| Status::internal(format!("Failed to calibrate actuator, {:?}", e)))?; + + Ok(Response::new(operation)) + } + + async fn get_actuators_state( + &self, + request: Request, + ) -> Result, Status> { + let request = request.into_inner(); + let actuator_ids = request.actuator_ids.clone(); + let states = self + .actuator + .get_actuators_state(actuator_ids) + .await + .map_err(|e| Status::internal(format!("Failed to get actuators state, {:?}", e)))?; + + let telemetry_states: Vec<_> = states.iter().map(ActuatorState::from).collect(); + let telemetry = Telemetry::get().await; + if let Some(telemetry) = telemetry { + if let Err(e) = telemetry.publish("actuator/state", &telemetry_states).await { + tracing::warn!("Failed to publish telemetry: {}", e); + } + } + + trace!( + "Getting actuators state, request: {:?}, response: {:?}", + request.clone(), + states + ); + Ok(Response::new(GetActuatorsStateResponse { states })) + } +} diff --git a/kos_core/src/services/imu.rs b/kos_core/src/services/imu.rs new file mode 100644 index 0000000..7ec47b0 --- /dev/null +++ b/kos_core/src/services/imu.rs @@ -0,0 +1,131 @@ +use crate::grpc_interface::google::longrunning::Operation; +use crate::hal::IMU; +use crate::kos_proto::common::ActionResponse; +use crate::kos_proto::imu::imu_service_server::ImuService; +use crate::kos_proto::imu::*; +use crate::telemetry::Telemetry; +use crate::telemetry_types::{EulerAngles, ImuValues, Quaternion}; +use eyre::OptionExt; +use std::sync::Arc; +use tonic::{Request, Response, Status}; +use tracing::trace; + +pub struct IMUServiceImpl { + imu: Arc, +} + +impl IMUServiceImpl { + pub fn new(imu: Arc) -> Self { + Self { imu } + } +} + +#[tonic::async_trait] +impl ImuService for IMUServiceImpl { + async fn get_values( + &self, + _request: Request<()>, + ) -> Result, Status> { + let values = self + .imu + .get_values() + .await + .map_err(|e| Status::internal(format!("Failed to get IMU values, {:?}", e)))?; + + let telemetry = Telemetry::get().await; + if let Some(telemetry) = telemetry { + if let Err(e) = telemetry + .publish("imu/values", &ImuValues::from(&values)) + .await + { + tracing::warn!("Failed to publish telemetry: {}", e); + } + } + + trace!("Getting IMU values, response: {:?}", values); + + Ok(Response::new(values)) + } + + async fn calibrate(&self, _request: Request<()>) -> Result, Status> { + let _status = self + .imu + .calibrate() + .await + .map_err(|e| Status::internal(format!("Failed to calibrate IMU, {:?}", e)))?; + + Ok(Response::new(Operation { + name: "operations/calibrate_imu/0".to_string(), + metadata: None, + done: false, + result: None, + })) + } + + async fn zero( + &self, + request: Request, + ) -> Result, Status> { + let duration = request + .into_inner() + .duration + .ok_or_eyre("Duration is required") + .map_err(|_| Status::internal("Failed to parse duration"))?; + + let duration = std::time::Duration::from_nanos(duration.nanos as u64) + + std::time::Duration::from_secs(duration.seconds as u64); + + let response = self + .imu + .zero(duration) + .await + .map_err(|e| Status::internal(format!("Failed to zero IMU, {:?}", e)))?; + Ok(Response::new(response)) + } + + async fn get_euler( + &self, + _request: Request<()>, + ) -> Result, Status> { + let euler = self + .imu + .get_euler() + .await + .map_err(|e| Status::internal(format!("Failed to get euler, {:?}", e)))?; + + let telemetry = Telemetry::get().await; + if let Some(telemetry) = telemetry { + if let Err(e) = telemetry + .publish("imu/euler", &EulerAngles::from(&euler)) + .await + { + tracing::warn!("Failed to publish telemetry: {}", e); + } + } + + Ok(Response::new(euler)) + } + + async fn get_quaternion( + &self, + _request: Request<()>, + ) -> Result, Status> { + let quaternion = self + .imu + .get_quaternion() + .await + .map_err(|e| Status::internal(format!("Failed to get quaternion, {:?}", e)))?; + + let telemetry = Telemetry::get().await; + if let Some(telemetry) = telemetry { + if let Err(e) = telemetry + .publish("imu/quaternion", &Quaternion::from(&quaternion)) + .await + { + tracing::warn!("Failed to publish telemetry: {}", e); + } + } + + Ok(Response::new(quaternion)) + } +} diff --git a/kos_core/src/services/mod.rs b/kos_core/src/services/mod.rs new file mode 100644 index 0000000..b88c42a --- /dev/null +++ b/kos_core/src/services/mod.rs @@ -0,0 +1,7 @@ +mod actuator; +mod imu; +mod operations; + +pub use actuator::*; +pub use imu::*; +pub use operations::*; diff --git a/kos_core/src/services/operations.rs b/kos_core/src/services/operations.rs new file mode 100644 index 0000000..f02d236 --- /dev/null +++ b/kos_core/src/services/operations.rs @@ -0,0 +1,190 @@ +use crate::grpc_interface::google::longrunning::{ + operations_server::Operations, CancelOperationRequest, DeleteOperationRequest, + GetOperationRequest, ListOperationsRequest, ListOperationsResponse, Operation as LroOperation, + WaitOperationRequest, +}; +use crate::hal::Operation; +use prost::Message; +use prost_types::Any; +use std::collections::HashMap; +use std::sync::Arc; +use tokio::sync::Mutex; +use tonic::async_trait; +use tonic::{Request, Response, Status}; + +pub struct OperationsServiceImpl { + pub operation_store: Arc>>, +} + +impl OperationsServiceImpl { + pub fn new(operation_store: Arc>>) -> Self { + Self { operation_store } + } + + pub async fn create( + &self, + name: String, + metadata: T, + type_url: &str, + ) -> Result { + let mut buf = Vec::new(); + metadata + .encode(&mut buf) + .map_err(|e| Status::internal(format!("Failed to encode metadata: {}", e)))?; + + let operation = LroOperation { + name: name.clone(), + metadata: Some(Any { + type_url: type_url.to_string(), + value: buf, + }), + done: false, + result: None, + }; + + self.operation_store + .lock() + .await + .insert(name, operation.clone()); + + Ok(operation) + } + + pub async fn get_metadata( + &self, + name: &str, + ) -> Result, Status> { + let store = self.operation_store.lock().await; + if let Some(operation) = store.get(name) { + if let Some(metadata) = &operation.metadata { + return T::decode(&metadata.value[..]) + .map(Some) + .map_err(|e| Status::internal(format!("Failed to decode metadata: {}", e))); + } + } + Ok(None) + } + + pub async fn update_metadata( + &self, + name: &str, + metadata: T, + mark_done: bool, + ) -> Result<(), Status> { + let mut store = self.operation_store.lock().await; + + if let Some(operation) = store.get_mut(name) { + let mut buf = Vec::new(); + metadata + .encode(&mut buf) + .map_err(|e| Status::internal(format!("Failed to encode metadata: {}", e)))?; + + if let Some(existing_metadata) = &mut operation.metadata { + existing_metadata.value = buf; + if mark_done { + operation.done = true; + } + Ok(()) + } else { + Err(Status::internal("Operation has no metadata field")) + } + } else { + Err(Status::not_found("Operation not found")) + } + } +} + +impl Default for OperationsServiceImpl { + fn default() -> Self { + unimplemented!( + "Default is not implemented because OperationsServiceImpl requires external operations store" + ) + } +} + +#[tonic::async_trait] +impl Operations for OperationsServiceImpl { + async fn get_operation( + &self, + request: Request, + ) -> Result, Status> { + let name = request.into_inner().name; + let store = self.operation_store.lock().await; + if let Some(operation) = store.get(&name) { + Ok(Response::new(operation.clone())) + } else { + Err(Status::not_found("Operation not found")) + } + } + + // Implement other methods if needed + async fn list_operations( + &self, + _request: Request, + ) -> Result, Status> { + // Not implemented in this example + Err(Status::unimplemented("ListOperations is not implemented")) + } + + async fn cancel_operation( + &self, + _request: Request, + ) -> Result, Status> { + // Not implemented in this example + Err(Status::unimplemented("CancelOperation is not implemented")) + } + + async fn delete_operation( + &self, + _request: Request, + ) -> Result, Status> { + // Not implemented in this example + Err(Status::unimplemented("DeleteOperation is not implemented")) + } + + async fn wait_operation( + &self, + _request: Request, + ) -> Result, Status> { + // Not implemented in this example + Err(Status::unimplemented("WaitOperation is not implemented")) + } +} + +#[async_trait] +impl Operations for Arc { + async fn get_operation( + &self, + request: Request, + ) -> Result, Status> { + self.as_ref().get_operation(request).await + } + + async fn list_operations( + &self, + request: Request, + ) -> Result, Status> { + self.as_ref().list_operations(request).await + } + + async fn delete_operation( + &self, + request: Request, + ) -> Result, Status> { + self.as_ref().delete_operation(request).await + } + + async fn cancel_operation( + &self, + request: Request, + ) -> Result, Status> { + self.as_ref().cancel_operation(request).await + } + + async fn wait_operation( + &self, + request: Request, + ) -> Result, Status> { + self.as_ref().wait_operation(request).await + } +} diff --git a/kos_core/src/telemetry.rs b/kos_core/src/telemetry.rs new file mode 100644 index 0000000..f990b4d --- /dev/null +++ b/kos_core/src/telemetry.rs @@ -0,0 +1,64 @@ +// TODO: Implement telemetry. +// General idea - MQTT for the robot, where serial of the robot is a topic. +// Mosquitto is the broker which will pass messages to InfluxDB +// We log desired vs actual joint angles (torque/velocity/position if applicable), +// as well as IMU data. + +use eyre::Result; +use lazy_static::lazy_static; +use rumqttc::{AsyncClient, MqttOptions, QoS}; +use serde::Serialize; +use std::sync::Arc; +use tokio::sync::Mutex; + +#[derive(Clone)] +pub struct Telemetry { + client: Arc, + robot_id: String, +} + +lazy_static! { + static ref TELEMETRY: Arc>> = Arc::new(Mutex::new(None)); +} + +impl Telemetry { + pub async fn initialize(robot_id: &str, mqtt_host: &str, mqtt_port: u16) -> Result<()> { + let mut mqtt_options = MqttOptions::new(format!("kos-{}", robot_id), mqtt_host, mqtt_port); + mqtt_options.set_keep_alive(std::time::Duration::from_secs(5)); + + let (client, mut eventloop) = AsyncClient::new(mqtt_options, 10); + + // Spawn a task to handle MQTT connection events + tokio::spawn(async move { + while let Ok(notification) = eventloop.poll().await { + tracing::trace!("MQTT Event: {:?}", notification); + } + }); + + let telemetry = Telemetry { + client: Arc::new(client), + robot_id: robot_id.to_string(), + }; + + tracing::debug!("Initializing telemetry for robot {}", robot_id); + let mut global = TELEMETRY.lock().await; + *global = Some(telemetry); + + Ok(()) + } + + pub async fn get() -> Option { + TELEMETRY.lock().await.clone() + } + + pub async fn publish(&self, topic: &str, payload: &T) -> Result<()> { + let payload = serde_json::to_string(payload)?; + let full_topic = format!("robots/{}/{}", self.robot_id, topic); + + self.client + .publish(full_topic, QoS::AtLeastOnce, false, payload) + .await?; + + Ok(()) + } +} diff --git a/kos_core/src/telemetry_types.rs b/kos_core/src/telemetry_types.rs new file mode 100644 index 0000000..0024480 --- /dev/null +++ b/kos_core/src/telemetry_types.rs @@ -0,0 +1,118 @@ +use crate::grpc_interface::kos::actuator::{ + ActuatorCommand as ProtoActuatorCommand, ActuatorStateResponse, +}; +use crate::grpc_interface::kos::imu::{EulerAnglesResponse, ImuValuesResponse, QuaternionResponse}; +use serde::Serialize; + +#[derive(Serialize)] +pub struct ImuValues { + pub accel_x: f64, + pub accel_y: f64, + pub accel_z: f64, + pub gyro_x: f64, + pub gyro_y: f64, + pub gyro_z: f64, + pub mag_x: Option, + pub mag_y: Option, + pub mag_z: Option, + pub error: Option, +} + +#[derive(Serialize)] +pub struct EulerAngles { + pub roll: f64, + pub pitch: f64, + pub yaw: f64, +} + +#[derive(Serialize)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +#[derive(Serialize)] +pub struct ActuatorState { + pub actuator_id: u32, + pub online: bool, + pub position: Option, + pub velocity: Option, + pub torque: Option, + pub temperature: Option, + pub voltage: Option, + pub current: Option, +} + +#[derive(Clone, Debug, Serialize)] +pub struct ActuatorCommand { + pub actuator_id: u32, + pub position: Option, + pub velocity: Option, + pub torque: Option, +} + +impl From<&EulerAnglesResponse> for EulerAngles { + fn from(resp: &EulerAnglesResponse) -> Self { + Self { + roll: resp.roll, + pitch: resp.pitch, + yaw: resp.yaw, + } + } +} + +impl From<&ImuValuesResponse> for ImuValues { + fn from(resp: &ImuValuesResponse) -> Self { + Self { + accel_x: resp.accel_x, + accel_y: resp.accel_y, + accel_z: resp.accel_z, + gyro_x: resp.gyro_x, + gyro_y: resp.gyro_y, + gyro_z: resp.gyro_z, + mag_x: resp.mag_x, + mag_y: resp.mag_y, + mag_z: resp.mag_z, + error: resp.error.as_ref().map(|e| e.message.clone()), + } + } +} + +impl From<&QuaternionResponse> for Quaternion { + fn from(resp: &QuaternionResponse) -> Self { + Self { + x: resp.x, + y: resp.y, + z: resp.z, + w: resp.w, + } + } +} + +impl From<&ActuatorStateResponse> for ActuatorState { + fn from(resp: &ActuatorStateResponse) -> Self { + Self { + actuator_id: resp.actuator_id, + online: resp.online, + position: resp.position, + velocity: resp.velocity, + torque: resp.torque, + temperature: resp.temperature, + voltage: resp.voltage, + current: resp.current, + } + } +} + +impl From<&ProtoActuatorCommand> for ActuatorCommand { + fn from(cmd: &ProtoActuatorCommand) -> Self { + Self { + actuator_id: cmd.actuator_id, + position: cmd.position, + velocity: cmd.velocity, + torque: cmd.torque, + } + } +} diff --git a/platforms/kscale_micro/Cargo.toml b/platforms/kscale_micro/Cargo.toml new file mode 100644 index 0000000..bb4543d --- /dev/null +++ b/platforms/kscale_micro/Cargo.toml @@ -0,0 +1,4 @@ +[package] +name = "kscale_micro" +version = "0.1.0" +edition = "2021" diff --git a/platforms/kscale_micro/src/lib.rs b/platforms/kscale_micro/src/lib.rs new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/platforms/kscale_micro/src/lib.rs @@ -0,0 +1 @@ + diff --git a/platforms/kscale_pro/Cargo.toml b/platforms/kscale_pro/Cargo.toml new file mode 100644 index 0000000..1d430a1 --- /dev/null +++ b/platforms/kscale_pro/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "kscale_pro" +version = "0.1.0" +edition = "2021" + +[dependencies] +eyre = "0.6" +robstride = "0.2.8" +imu = "0.1.4" \ No newline at end of file diff --git a/platforms/kscale_pro/src/actuator.rs b/platforms/kscale_pro/src/actuator.rs new file mode 100644 index 0000000..7b73ccd --- /dev/null +++ b/platforms/kscale_pro/src/actuator.rs @@ -0,0 +1,84 @@ +use async_trait::async_trait; +use eyre::Result; +use kos_core::google_proto::longrunning::Operation; +use kos_core::services::OperationsServiceImpl; +use kos_core::{ + hal::{ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorRequest}, + kos_proto::{actuator::*, common::ActionResult}, +}; +use std::collections::HashMap; +use std::sync::Arc; + +use robstride::{MotorMode, MotorType, Motors, MotorsSupervisor}; + +pub struct KscaleProActuator { + motors_supervisor: MotorsSupervisor, +} + +impl KscaleProActuator { + pub fn new( + port: &str, + motor_infos: HashMap, + verbose: Option, + max_update_rate: Option, + zero_on_init: Option, + ) -> Self { + + let motors_supervisor = MotorsSupervisor::new( + port, + &motor_infos, + verbose.unwrap_or(false), + max_update_rate.unwrap_or(100000), + zero_on_init.unwrap_or(false), + ) + .unwrap(); + + KscaleProActuator { + motors_supervisor, + } + } +} + +#[async_trait] +impl Actuator for KscaleProActuator { + async fn command_actuators( + &self, + _commands: Vec, + ) -> Result> { + Ok(vec![]) + } + + async fn configure_actuator( + &self, + _config: ConfigureActuatorRequest, + ) -> Result { + Ok(ActionResponse { + success: true, + error: None, + }) + } + + async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result { + Ok(Operation::default()) + } + + async fn get_actuators_state( + &self, + _actuator_ids: Vec, + ) -> Result> { + let feedback = self.motors_supervisor.get_latest_feedback(); + Ok(feedback + .iter() + .map(|(id, state)| ActuatorStateResponse { + actuator_id: *id, + online: state.mode==MotorMode.Motor, + position: state.position, + velocity: state.velocity, + torque: state.torque, + temperature: None, + voltage: None, + current: None, + }) + .collect()) + } +} diff --git a/platforms/kscale_pro/src/hexmove.rs b/platforms/kscale_pro/src/hexmove.rs new file mode 100644 index 0000000..8fa7b6e --- /dev/null +++ b/platforms/kscale_pro/src/hexmove.rs @@ -0,0 +1,80 @@ + +use kos_core::{ + hal::{ + CalibrateImuMetadata, CalibrationStatus, EulerAnglesResponse, ImuValuesResponse, + QuaternionResponse, IMU, Operation, + }, + kos_proto::common::ActionResponse, +}; + +use imu::hexmove::*; + +pub struct KscaleProIMU { + 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 Default for KscaleProIMU { + fn default() -> Self { + unimplemented!("KscaleProIMU cannot be default, it requires an operations store") + } +} + +#[async_trait] +impl IMU for StubIMU { + async fn get_values(&self) -> Result { + let data = self.imu.get_data(); + Ok(ImuValuesResponse { + accel_x: None, + accel_y: None, + accel_z: None, + gyro_x: None, + gyro_y: None, + gyro_z: None, + mag_x: None, + mag_y: None, + mag_z: None, + error: None, + }) + } + + async fn calibrate(&self) -> Result { + Ok(Operation::default()) + } + + async fn zero(&self, _duration: Duration) -> Result { + Ok(ActionResponse { + success: true, + error: None, + }) + } + + async fn get_euler(&self) -> Result { + let data = self.imu.get_data(); + Ok(EulerAnglesResponse { + roll: data.x_angle, + pitch: data.y_angle, + yaw: data.z_angle, + error: None, + }) + } + + async fn get_quaternion(&self) -> Result { + Ok(QuaternionResponse { + w: 1.0, + x: 0.0, + y: 0.0, + z: 0.0, + error: None, + }) + } +} diff --git a/platforms/kscale_pro/src/lib.rs b/platforms/kscale_pro/src/lib.rs new file mode 100644 index 0000000..0e6898f --- /dev/null +++ b/platforms/kscale_pro/src/lib.rs @@ -0,0 +1,59 @@ +mod actuator; +mod hexmove; + +pub use actuator::*; +pub use hexmove::*; + +use kos_core::hal::Operation; +use kos_core::kos_proto::{ + actuator::actuator_service_server::ActuatorServiceServer, + imu::imu_service_server::ImuServiceServer, +}; +use kos_core::services::{ActuatorServiceImpl, IMUServiceImpl}; +use kos_core::{services::OperationsServiceImpl, Platform, ServiceEnum}; +use std::sync::Arc; + +pub struct KscaleProPlatform {} + +impl KscaleProPlatform { + pub fn new() -> Self { + Self {} + } +} + +impl Default for KscaleProPlatform { + fn default() -> Self { + KscaleProPlatform::new() + } +} + +impl Platform for KscaleProPlatform { + fn name(&self) -> &'static str { + "Kscale Pro" + } + + fn initialize(&mut self, _operations_service: Arc) -> eyre::Result<()> { + // Initialize the platform + Ok(()) + } + + fn create_services(&self, operations_service: Arc) -> Vec { + // Add available services here + vec![ + ServiceEnum::Imu(ImuServiceServer::new(IMUServiceImpl::new(Arc::new( + KscaleProIMU::new("can0", 1, 1), + )))), + ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( + Arc::new(KscaleProActuator::new( + "/dev/ttyCH341USB0", + HashMap::new() + )), + ))), + ] + } + + fn shutdown(&mut self) -> eyre::Result<()> { + // Shutdown and cleanup code goes here + Ok(()) + } +} diff --git a/platforms/sim/Cargo.toml b/platforms/sim/Cargo.toml new file mode 100644 index 0000000..e51f840 --- /dev/null +++ b/platforms/sim/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "sim" +version = "0.1.0" +edition = "2021" + +[dependencies] +kos_core = { path = "../../kos_core" } +async-trait = "0.1" +eyre = "0.6" diff --git a/platforms/sim/src/imu.rs b/platforms/sim/src/imu.rs new file mode 100644 index 0000000..f7c9d71 --- /dev/null +++ b/platforms/sim/src/imu.rs @@ -0,0 +1,45 @@ +use async_trait::async_trait; +use eyre::Result; +use kos_core::{ + google_proto::longrunning::Operation, + hal::{EulerAnglesResponse, ImuValuesResponse, QuaternionResponse, IMU}, + kos_proto::common::ActionResponse, +}; +use std::time::Duration; + +pub struct SimIMU {} + +impl SimIMU { + pub fn new() -> Self { + SimIMU {} + } +} + +impl Default for SimIMU { + fn default() -> Self { + SimIMU::new() + } +} + +#[async_trait] +impl IMU for SimIMU { + async fn get_values(&self) -> Result { + todo!() + } + + async fn calibrate(&self) -> Result { + todo!() + } + + async fn zero(&self, _duration: Duration) -> Result { + todo!() + } + + async fn get_euler(&self) -> Result { + todo!() + } + + async fn get_quaternion(&self) -> Result { + todo!() + } +} diff --git a/platforms/sim/src/lib.rs b/platforms/sim/src/lib.rs new file mode 100644 index 0000000..ceaf720 --- /dev/null +++ b/platforms/sim/src/lib.rs @@ -0,0 +1,3 @@ +mod imu; + +pub use imu::*; diff --git a/platforms/stub/Cargo.toml b/platforms/stub/Cargo.toml new file mode 100644 index 0000000..85f62d5 --- /dev/null +++ b/platforms/stub/Cargo.toml @@ -0,0 +1,14 @@ +[package] +name = "stub" +version = "0.1.0" +edition = "2021" + +[dependencies] +kos_core = { path = "../../kos_core" } +async-trait = "0.1" +eyre = "0.6" +tokio = { version = "1", features = ["full"] } +uuid = { version = "1", features = ["v4"] } +prost-types = "0.13" +prost = "0.13" +tracing = "0.1" \ No newline at end of file diff --git a/platforms/stub/src/actuator.rs b/platforms/stub/src/actuator.rs new file mode 100644 index 0000000..aceab68 --- /dev/null +++ b/platforms/stub/src/actuator.rs @@ -0,0 +1,130 @@ +use async_trait::async_trait; +use eyre::Result; +use kos_core::google_proto::longrunning::Operation; +use kos_core::services::OperationsServiceImpl; +use kos_core::{ + hal::{ + ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorMetadata, + CalibrateActuatorRequest, CalibrationStatus, + }, + kos_proto::{actuator::*, common::ActionResult}, +}; +use std::sync::mpsc::{channel, Sender}; +use std::sync::Arc; +use std::thread; +use tokio::runtime::Runtime; +use tokio::time::Duration; +use tracing::debug; + +pub struct StubActuator { + operations: Arc, + calibration_tx: Sender, +} + +impl StubActuator { + pub fn new(operations: Arc) -> Self { + let (tx, rx) = channel::(); + + // Spawn the calibration thread + let operations_clone = operations.clone(); + thread::spawn(move || { + // Create a new runtime for this thread + let rt = Runtime::new().expect("Failed to create runtime"); + + loop { + // Wait for actuator IDs to calibrate + if let Ok(actuator_id) = rx.recv() { + let ops = operations_clone.clone(); + debug!("Calibrating actuator ID: {}", actuator_id); + + // Sleep for 15 seconds to simulate calibration + thread::sleep(Duration::from_secs(15)); + debug!("Calibrated actuator ID: {}", actuator_id); + + // Update the operation status + let operation_name = format!("operations/calibrate_actuator/{:?}", actuator_id); + debug!("Updating operation status for: {}", operation_name); + + let metadata = CalibrateActuatorMetadata { + actuator_id, + status: CalibrationStatus::Calibrated.to_string(), + }; + + if let Err(e) = + rt.block_on(ops.update_metadata(&operation_name, metadata, true)) + { + debug!("Failed to update calibration status: {}", e); + } + + debug!("Updated operation status for: {}", operation_name); + } + } + }); + + StubActuator { + operations, + calibration_tx: tx, + } + } +} + +#[async_trait] +impl Actuator for StubActuator { + async fn command_actuators( + &self, + _commands: Vec, + ) -> Result> { + Ok(vec![]) + } + + async fn configure_actuator( + &self, + _config: ConfigureActuatorRequest, + ) -> Result { + Ok(ActionResponse { + success: true, + error: None, + }) + } + + async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result { + let metadata = CalibrateActuatorMetadata { + actuator_id: request.actuator_id, + status: CalibrationStatus::Calibrating.to_string(), + }; + + let name = format!("operations/calibrate_actuator/{:?}", request.actuator_id); + let operation = self + .operations + .create( + name, + metadata, + "type.googleapis.com/kos.actuator.CalibrateActuatorMetadata", + ) + .await + .map_err(|e| eyre::eyre!("Failed to create operation: {}", e))?; + + // Send actuator ID to calibration thread + self.calibration_tx + .send(request.actuator_id) + .map_err(|e| eyre::eyre!("Failed to start calibration: {}", e))?; + + Ok(operation) + } + + async fn get_actuators_state( + &self, + _actuator_ids: Vec, + ) -> Result> { + Ok(vec![ActuatorStateResponse { + actuator_id: 1, + online: true, + position: Some(0.0), + velocity: Some(0.0), + torque: Some(0.0), + temperature: Some(0.0), + voltage: Some(0.0), + current: Some(0.0), + }]) + } +} diff --git a/platforms/stub/src/imu.rs b/platforms/stub/src/imu.rs new file mode 100644 index 0000000..ab94afa --- /dev/null +++ b/platforms/stub/src/imu.rs @@ -0,0 +1,96 @@ +use crate::Operation; +use async_trait::async_trait; +use eyre::Result; +use kos_core::services::OperationsServiceImpl; +use kos_core::{ + hal::{ + CalibrateImuMetadata, CalibrationStatus, EulerAnglesResponse, ImuValuesResponse, + QuaternionResponse, IMU, + }, + kos_proto::common::ActionResponse, +}; +use std::sync::Arc; +use std::time::Duration; +use uuid::Uuid; + +pub struct StubIMU { + operations_service: Arc, +} + +impl StubIMU { + pub fn new(operations_service: Arc) -> Self { + StubIMU { operations_service } + } +} + +impl Default for StubIMU { + fn default() -> Self { + unimplemented!("StubIMU cannot be default, it requires an operations store") + } +} + +#[async_trait] +impl IMU for StubIMU { + async fn get_values(&self) -> Result { + Ok(ImuValuesResponse { + accel_x: 1.0, + accel_y: 2.0, + accel_z: 3.0, + gyro_x: 0.0, + gyro_y: 0.0, + gyro_z: 0.0, + mag_x: None, + mag_y: None, + mag_z: None, + error: None, + }) + } + + async fn calibrate(&self) -> Result { + let operation = Operation { + name: format!("operations/imu/calibrate/{}", Uuid::new_v4()), + metadata: None, + done: false, + result: None, + }; + let metadata = CalibrateImuMetadata { + status: CalibrationStatus::Calibrating.to_string(), + }; + let operation = self + .operations_service + .create( + operation.name, + metadata, + "type.googleapis.com/kos.imu.CalibrateIMUMetadata", + ) + .await?; + + Ok(operation) + } + + async fn zero(&self, _duration: Duration) -> Result { + Ok(ActionResponse { + success: true, + error: None, + }) + } + + async fn get_euler(&self) -> Result { + Ok(EulerAnglesResponse { + roll: 0.0, + pitch: 30.0, + yaw: 0.0, + error: None, + }) + } + + async fn get_quaternion(&self) -> Result { + Ok(QuaternionResponse { + w: 1.0, + x: 0.0, + y: 0.0, + z: 0.0, + error: None, + }) + } +} diff --git a/platforms/stub/src/lib.rs b/platforms/stub/src/lib.rs new file mode 100644 index 0000000..7276b15 --- /dev/null +++ b/platforms/stub/src/lib.rs @@ -0,0 +1,56 @@ +mod actuator; +mod imu; + +pub use actuator::*; +pub use imu::*; + +use kos_core::hal::Operation; +use kos_core::kos_proto::{ + actuator::actuator_service_server::ActuatorServiceServer, + imu::imu_service_server::ImuServiceServer, +}; +use kos_core::services::{ActuatorServiceImpl, IMUServiceImpl}; +use kos_core::{services::OperationsServiceImpl, Platform, ServiceEnum}; +use std::sync::Arc; + +pub struct StubPlatform {} + +impl StubPlatform { + pub fn new() -> Self { + Self {} + } +} + +impl Default for StubPlatform { + fn default() -> Self { + StubPlatform::new() + } +} + +impl Platform for StubPlatform { + fn name(&self) -> &'static str { + "Stub" + } + + fn initialize(&mut self, _operations_service: Arc) -> eyre::Result<()> { + // Initialize the platform + Ok(()) + } + + fn create_services(&self, operations_service: Arc) -> Vec { + // Add available services here + 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<()> { + // Shutdown and cleanup code goes here + Ok(()) + } +} diff --git a/proto/googleapis b/proto/googleapis new file mode 160000 index 0000000..c72f219 --- /dev/null +++ b/proto/googleapis @@ -0,0 +1 @@ +Subproject commit c72f219fedbb57d3f83c10550e135c4824b670eb diff --git a/proto/kos/actuator.proto b/proto/kos/actuator.proto new file mode 100644 index 0000000..cec6c11 --- /dev/null +++ b/proto/kos/actuator.proto @@ -0,0 +1,102 @@ +syntax = "proto3"; + +package kos.actuator; + +import "google/protobuf/empty.proto"; +import "google/longrunning/operations.proto"; +import "kos/common.proto"; + +option go_package = "kos/actuator;actuator"; +option java_package = "com.kos.actuator"; +option csharp_namespace = "KOS.Actuator"; + +// The ActuatorService provides methods to control and monitor actuators. +service ActuatorService { + // Commands multiple actuators at once. + rpc CommandActuators(CommandActuatorsRequest) returns (CommandActuatorsResponse); + + // Configures an actuator's parameters. + rpc ConfigureActuator(ConfigureActuatorRequest) returns (kos.common.ActionResponse); + + // Calibrates an actuator (long-running operation). + rpc CalibrateActuator(CalibrateActuatorRequest) returns (google.longrunning.Operation) { + option (google.longrunning.operation_info) = { + response_type: "CalibrateActuatorResponse" + metadata_type: "CalibrateActuatorMetadata" + }; + } + + // Retrieves the state of multiple actuators. + rpc GetActuatorsState(GetActuatorsStateRequest) returns (GetActuatorsStateResponse); +} + +// Message representing a command to an actuator. +message ActuatorCommand { + uint32 actuator_id = 1; // Actuator ID + optional double position = 2; // Desired position in degrees + optional double velocity = 3; // Desired velocity in degrees/second + optional double torque = 4; // Desired torque in Nm +} + +// Request message for CommandActuators. +message CommandActuatorsRequest { + repeated ActuatorCommand commands = 1; // List of actuator commands +} + +// Response message for CommandActuators. +message CommandActuatorsResponse { + repeated kos.common.ActionResult results = 1; // Results per actuator +} + +// Request message for ConfigureActuator. +message ConfigureActuatorRequest { + uint32 actuator_id = 1; // Actuator ID + optional double kp = 2; // Proportional gain + optional double kd = 3; // Derivative gain + optional double ki = 4; // Integral gain + optional double max_torque = 5; // Max torque (%) + optional double protective_torque = 6; // Protective torque (%) + optional float protection_time = 7; // Protection time in seconds + optional bool torque_enabled = 8; // Torque enabled flag +} + +// Request message for CalibrateActuator. +message CalibrateActuatorRequest { + uint32 actuator_id = 1; // Actuator ID + optional double calibration_speed = 2; // Calibration speed in degrees/second + optional float threshold_current = 3; // Threshold current in amperes +} + +// Response message for CalibrateActuator operation. +message CalibrateActuatorResponse { + uint32 actuator_id = 1; // Actuator ID + kos.common.Error error = 2; // Error details if calibration failed +} + +// Metadata for CalibrateActuator operation. +message CalibrateActuatorMetadata { + uint32 actuator_id = 1; // Actuator ID + string status = 2; // Status ("IN_PROGRESS", "SUCCEEDED", "FAILED") +} + +// Request message for GetActuatorsState. +message GetActuatorsStateRequest { + repeated uint32 actuator_ids = 1; // Actuator IDs to query +} + +// Response message containing actuator states. +message GetActuatorsStateResponse { + repeated ActuatorStateResponse states = 1; // List of actuator states +} + +// State information for a single actuator. +message ActuatorStateResponse { + uint32 actuator_id = 1; // Actuator ID + bool online = 2; // Online status + optional double position = 3; // Position in degrees + optional double velocity = 4; // Velocity in degrees/second + optional double torque = 5; // Torque in Nm + optional double temperature = 6; // Temperature in Celsius + optional float voltage = 7; // Voltage in volts + optional float current = 8; // Current in amperes +} diff --git a/proto/kos/common.proto b/proto/kos/common.proto new file mode 100644 index 0000000..ff284cc --- /dev/null +++ b/proto/kos/common.proto @@ -0,0 +1,36 @@ +syntax = "proto3"; + +package kos.common; + +option go_package = "kos/common;common"; +option java_package = "com.kos.common"; +option csharp_namespace = "KOS.Common"; + +// Common error codes +enum ErrorCode { + UNKNOWN = 0; + NOT_IMPLEMENTED = 1; + INVALID_ARGUMENT = 2; + HARDWARE_FAILURE = 3; + TIMEOUT = 4; + UNAUTHORIZED = 5; +} + +// Common error message +message Error { + ErrorCode code = 1; // Error code + string message = 2; // Error message +} + +// Common action response +message ActionResponse { + bool success = 1; // Indicates if the action was successful + Error error = 2; // Error details if the action failed +} + +// Common result for per-actuator actions +message ActionResult { + uint32 actuator_id = 1; // Actuator ID + bool success = 2; // Indicates if the action was successful + Error error = 3; // Error details if the action failed +} diff --git a/proto/kos/imu.proto b/proto/kos/imu.proto new file mode 100644 index 0000000..734fac9 --- /dev/null +++ b/proto/kos/imu.proto @@ -0,0 +1,81 @@ +syntax = "proto3"; + +package kos.imu; + +import "google/protobuf/empty.proto"; +import "google/protobuf/duration.proto"; +import "google/longrunning/operations.proto"; +import "kos/common.proto"; + +option go_package = "kos/imu;imu"; +option java_package = "com.kos.imu"; +option csharp_namespace = "KOS.IMU"; + +// The IMUService provides methods to interact with the Inertial Measurement Unit. +service IMUService { + // Retrieves the latest IMU sensor values. + rpc GetValues(google.protobuf.Empty) returns (IMUValuesResponse); + + // Calibrates the IMU (long-running operation). + rpc Calibrate(google.protobuf.Empty) returns (google.longrunning.Operation) { + option (google.longrunning.operation_info) = { + response_type: "CalibrateIMUResponse" + metadata_type: "CalibrateIMUMetadata" + }; + } + + // Zeros the IMU readings. + rpc Zero(ZeroIMURequest) returns (kos.common.ActionResponse); + + // Retrieves Euler angles from the IMU. + rpc GetEuler(google.protobuf.Empty) returns (EulerAnglesResponse); + + // Retrieves quaternion from the IMU. + rpc GetQuaternion(google.protobuf.Empty) returns (QuaternionResponse); +} + +// Response message containing IMU values. +message IMUValuesResponse { + double accel_x = 1; // Acceleration X-axis + double accel_y = 2; // Acceleration Y-axis + double accel_z = 3; // Acceleration Z-axis + double gyro_x = 4; // Gyroscope X-axis + double gyro_y = 5; // Gyroscope Y-axis + double gyro_z = 6; // Gyroscope Z-axis + optional double mag_x = 7; // Magnetometer X-axis + optional double mag_y = 8; // Magnetometer Y-axis + optional double mag_z = 9; // Magnetometer Z-axis + kos.common.Error error = 10; // Error details if any +} + +// Response message for Calibrate IMU operation. +message CalibrateIMUResponse { + kos.common.Error error = 1; // Error details if calibration failed +} + +// Metadata for Calibrate IMU operation. +message CalibrateIMUMetadata { + string status = 1; // Status ("IN_PROGRESS", "SUCCEEDED", "FAILED") +} + +// Request message for Zero IMU. +message ZeroIMURequest { + google.protobuf.Duration duration = 1; // Duration for zeroing +} + +// Response message containing Euler angles. +message EulerAnglesResponse { + double roll = 1; // Roll angle + double pitch = 2; // Pitch angle + double yaw = 3; // Yaw angle + kos.common.Error error = 4; // Error details if any +} + +// Response message containing quaternion. +message QuaternionResponse { + double x = 1; // Quaternion X component + double y = 2; // Quaternion Y component + double z = 3; // Quaternion Z component + double w = 4; // Quaternion W component + kos.common.Error error = 5; // Error details if any +} diff --git a/proto/kos/inference.proto b/proto/kos/inference.proto new file mode 100644 index 0000000..e4ed25c --- /dev/null +++ b/proto/kos/inference.proto @@ -0,0 +1,42 @@ +syntax = "proto3"; + +package kos.inference; + +import "google/protobuf/empty.proto"; +import "kos/common.proto"; + +option go_package = "kos/inference;inference"; +option java_package = "com.kos.inference"; +option csharp_namespace = "KOS.Inference"; + +// The InferenceService allows uploading models and running inference. +service InferenceService { + // Uploads a model to the robot. + rpc UploadModel(UploadModelRequest) returns (UploadModelResponse); + + // Runs inference using a specified model. + rpc Forward(ForwardRequest) returns (ForwardResponse); +} + +// Request message for uploading a model. +message UploadModelRequest { + bytes model = 1; // Model binary data +} + +// Response message containing the uploaded model's UID. +message UploadModelResponse { + string model_uid = 1; // Unique identifier for the model + kos.common.Error error = 2; // Error details if upload failed +} + +// Request message for running inference. +message ForwardRequest { + string model_uid = 1; // Model UID to use for inference + repeated float inputs = 2; // Input data for the model +} + +// Response message containing inference results. +message ForwardResponse { + repeated float outputs = 1; // Output data from the model + kos.common.Error error = 2; // Error details if inference failed +} diff --git a/proto/kos/process_manager.proto b/proto/kos/process_manager.proto new file mode 100644 index 0000000..3ebdb4c --- /dev/null +++ b/proto/kos/process_manager.proto @@ -0,0 +1,19 @@ +syntax = "proto3"; + +package kos.processmanager; + +import "google/protobuf/empty.proto"; +import "kos/common.proto"; + +option go_package = "kos/processmanager;processmanager"; +option java_package = "com.kos.processmanager"; +option csharp_namespace = "KOS.ProcessManager"; + +// The ProcessManagerService manages processes like video streaming. +service ProcessManagerService { + // Starts video streaming. + rpc StartVideoStreaming(google.protobuf.Empty) returns (kos.common.ActionResponse); + + // Stops video streaming. + rpc StopVideoStreaming(google.protobuf.Empty) returns (kos.common.ActionResponse); +} diff --git a/proto/kos/system.proto b/proto/kos/system.proto new file mode 100644 index 0000000..743a51a --- /dev/null +++ b/proto/kos/system.proto @@ -0,0 +1,92 @@ +syntax = "proto3"; + +package kos.system; + +import "google/protobuf/empty.proto"; +import "google/protobuf/timestamp.proto"; +import "google/longrunning/operations.proto"; +import "kos/common.proto"; + +option go_package = "kos/system;system"; +option java_package = "com.kos.system"; +option csharp_namespace = "KOS.System"; + +// The SystemService provides methods to interact with system-level functions. +service SystemService { + // Retrieves IP addresses of network interfaces. + rpc GetIPAddresses(google.protobuf.Empty) returns (GetIPAddressesResponse); + + // Sets Wi-Fi credentials. + rpc SetWiFiCredentials(SetWiFiCredentialsRequest) returns (kos.common.ActionResponse); + + // Retrieves system information. + rpc GetSystemInfo(google.protobuf.Empty) returns (GetSystemInfoResponse); + + // Retrieves diagnostic logs. + rpc GetDiagnosticLogs(GetDiagnosticLogsRequest) returns (GetDiagnosticLogsResponse); + + // Uploads an OTA update (long-running operation). + rpc UploadOTA(UploadOTARequest) returns (google.longrunning.Operation) { + option (google.longrunning.operation_info) = { + response_type: "UploadOTAResponse" + metadata_type: "UploadOTAMetadata" + }; + } +} + +// Response message containing IP addresses. +message GetIPAddressesResponse { + repeated NetworkInterface interfaces = 1; // Network interfaces and their IPs + kos.common.Error error = 2; // Error details if any +} + +// Network interface information. +message NetworkInterface { + string name = 1; // Interface name + repeated string ip_addresses = 2; // List of IP addresses +} + +// Request message for setting Wi-Fi credentials. +message SetWiFiCredentialsRequest { + string ssid = 1; // Wi-Fi SSID + string password = 2; // Wi-Fi password + // Additional fields for enterprise networks can be added here +} + +// Response message containing system information. +message GetSystemInfoResponse { + optional uint64 total_ram = 1; // Total RAM in bytes + optional uint64 used_ram = 2; // Used RAM in bytes + optional uint64 total_disk = 3; // Total disk space in bytes + optional uint64 used_disk = 4; // Used disk space in bytes + optional float cpu_usage = 5; // CPU usage percentage + optional float npu_usage = 6; // NPU usage percentage + kos.common.Error error = 7; // Error details if any +} + +// Request message for retrieving diagnostic logs. +message GetDiagnosticLogsRequest { + google.protobuf.Timestamp start_time = 1; // Start time for logs + google.protobuf.Timestamp end_time = 2; // End time for logs +} + +// Response message containing diagnostic logs. +message GetDiagnosticLogsResponse { + bytes logs = 1; // Logs data (e.g., tar.xz file) + kos.common.Error error = 2; // Error details if any +} + +// Request message for uploading an OTA update. +message UploadOTARequest { + bytes ota_file = 1; // OTA update file data +} + +// Response message for UploadOTA operation. +message UploadOTAResponse { + kos.common.Error error = 1; // Error details if upload failed +} + +// Metadata for UploadOTA operation. +message UploadOTAMetadata { + string status = 1; // Status ("IN_PROGRESS", "SUCCEEDED", "FAILED") +} diff --git a/Makefile b/pykos/Makefile similarity index 66% rename from Makefile rename to pykos/Makefile index 71608d7..445baa1 100644 --- a/Makefile +++ b/pykos/Makefile @@ -5,8 +5,8 @@ K-Scale Operating System # Installing -1. Create a new Conda environment: `conda create --name kos python=3.11` -2. Activate the environment: `conda activate kos` +1. Create a new Conda environment: `conda create --name pykos python=3.11` +2. Activate the environment: `conda activate pykos` 3. Install the package: `make install-dev` # Running Tests @@ -22,6 +22,23 @@ all: @echo "$$HELP_MESSAGE" .PHONY: all +# ------------------------ # +# Protobuf Generation # +# ------------------------ # + +generate-proto: + python -m grpc_tools.protoc --python_out=. --grpc_python_out=. --proto_path=../proto --proto_path=../proto/googleapis ../proto/kos/* +.PHONY: generate-proto + + +# ------------------------ # +# Build # +# ------------------------ # + +install-dev: + @pip install --verbose -e '.[dev]' +.PHONY: install-dev + # ------------------------ # # PyPI Build # # ------------------------ # diff --git a/pykos/README.md b/pykos/README.md new file mode 100644 index 0000000..67635f9 --- /dev/null +++ b/pykos/README.md @@ -0,0 +1,3 @@ +# pykos + +Python client for the KOS. \ No newline at end of file diff --git a/kos/py.typed b/pykos/kos/__init__.py similarity index 100% rename from kos/py.typed rename to pykos/kos/__init__.py diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py new file mode 100644 index 0000000..135f791 --- /dev/null +++ b/pykos/pykos/__init__.py @@ -0,0 +1,3 @@ +__version__ = "0.1.1" + +from pykos.client import KOS \ No newline at end of file diff --git a/pykos/pykos/client.py b/pykos/pykos/client.py new file mode 100644 index 0000000..c0c2b10 --- /dev/null +++ b/pykos/pykos/client.py @@ -0,0 +1,27 @@ +import grpc + +from pykos.services.imu import IMUServiceClient +from pykos.services.actuator import ActuatorServiceClient +class KOS: + """ + KOS client + + Args: + ip (str, optional): IP address of the robot running KOS. Defaults to localhost. + port (int, optional): Port of the robot running KOS. Defaults to 50051. + + Attributes: + imu (IMUServiceClient): Client for the IMU service. + """ + def __init__(self, ip: str = "localhost", port: int = 50051): + self.ip = ip + self.port = port + self.channel = grpc.insecure_channel(f"{self.ip}:{self.port}") + self.imu = IMUServiceClient(self.channel) + self.actuator = ActuatorServiceClient(self.channel) + + def close(self): + """ + Close the gRPC channel. + """ + self.channel.close() diff --git a/pykos/pykos/requirements-dev.txt b/pykos/pykos/requirements-dev.txt new file mode 100644 index 0000000..b591669 --- /dev/null +++ b/pykos/pykos/requirements-dev.txt @@ -0,0 +1,3 @@ +# requirements-dev.txt + +grpcio-tools diff --git a/pykos/pykos/requirements.txt b/pykos/pykos/requirements.txt new file mode 100644 index 0000000..f3771d1 --- /dev/null +++ b/pykos/pykos/requirements.txt @@ -0,0 +1,3 @@ +grpcio +protobuf==5.27.2 +googleapis-common-protos diff --git a/pykos/pykos/services/actuator.py b/pykos/pykos/services/actuator.py new file mode 100644 index 0000000..c297b00 --- /dev/null +++ b/pykos/pykos/services/actuator.py @@ -0,0 +1,50 @@ +from kos import actuator_pb2_grpc, actuator_pb2 +from google.protobuf.empty_pb2 import Empty +from google.protobuf.any_pb2 import Any +from google.longrunning import operations_pb2, operations_pb2_grpc +from kos.actuator_pb2 import CalibrateActuatorMetadata + +class CalibrationStatus: + Calibrating = "calibrating" + Calibrated = "calibrated" + Timeout = "timeout" + +class CalibrationMetadata: + def __init__(self, metadata_any: Any): + self.actuator_id = None + self.status = None + self.decode_metadata(metadata_any) + + def decode_metadata(self, metadata_any: Any): + metadata = CalibrateActuatorMetadata() + if metadata_any.Is(CalibrateActuatorMetadata.DESCRIPTOR): + metadata_any.Unpack(metadata) + self.actuator_id = metadata.actuator_id + self.status = metadata.status + + def __str__(self): + return f"CalibrationMetadata(actuator_id={self.actuator_id}, status={self.status})" + + def __repr__(self): + return self.__str__() + +class ActuatorServiceClient: + def __init__(self, channel): + self.stub = actuator_pb2_grpc.ActuatorServiceStub(channel) + self.operations_stub = operations_pb2_grpc.OperationsStub(channel) + + def calibrate(self, actuator_id: int): + """ + Calibrate an actuator. + + Returns: + Operation: The operation for the calibration. + """ + response = self.stub.CalibrateActuator(actuator_pb2.CalibrateActuatorRequest(actuator_id=actuator_id)) + metadata = CalibrationMetadata(response.metadata) + return metadata + + def get_calibration_status(self, actuator_id: int): + response = self.operations_stub.GetOperation(operations_pb2.GetOperationRequest(name=f"operations/calibrate_actuator/{actuator_id}")) + metadata = CalibrationMetadata(response.metadata) + return metadata.status diff --git a/pykos/pykos/services/imu.py b/pykos/pykos/services/imu.py new file mode 100644 index 0000000..961d500 --- /dev/null +++ b/pykos/pykos/services/imu.py @@ -0,0 +1,34 @@ +from kos import imu_pb2_grpc, imu_pb2 +from google.protobuf.empty_pb2 import Empty + +class ImuValues: + def __init__(self, response: imu_pb2.IMUValuesResponse): + self.accel_x = response.accel_x + self.accel_y = response.accel_y + self.accel_z = response.accel_z + self.gyro_x = response.gyro_x + self.gyro_y = response.gyro_y + self.gyro_z = response.gyro_z + self.mag_x = response.mag_x if response.HasField("mag_x") else None + self.mag_y = response.mag_y if response.HasField("mag_y") else None + self.mag_z = response.mag_z if response.HasField("mag_z") else None + self.error = response.error if response.HasField("error") else None + def __str__(self): + return f"ImuValues(accel_x={self.accel_x}, accel_y={self.accel_y}, accel_z={self.accel_z}, gyro_x={self.gyro_x}, gyro_y={self.gyro_y}, gyro_z={self.gyro_z}, mag_x={self.mag_x}, mag_y={self.mag_y}, mag_z={self.mag_z}, error={self.error})" + def __repr__(self): + return self.__str__() + +class IMUServiceClient: + def __init__(self, channel): + self.stub = imu_pb2_grpc.IMUServiceStub(channel) + + def get_imu_values(self): + """ + Get the latest IMU sensor values. + + Returns: + ImuValuesResponse: The latest IMU sensor values. + """ + response = self.stub.GetValues(Empty()) + return ImuValues(response) + \ No newline at end of file diff --git a/pyproject.toml b/pykos/pyproject.toml similarity index 97% rename from pyproject.toml rename to pykos/pyproject.toml index 6dd28ae..f68982b 100644 --- a/pyproject.toml +++ b/pykos/pyproject.toml @@ -67,7 +67,7 @@ dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" [tool.ruff.lint.isort] -known-first-party = ["kos", "tests"] +known-first-party = ["pykos", "tests"] combine-as-imports = true [tool.ruff.lint.pydocstyle] diff --git a/pykos/setup.py b/pykos/setup.py new file mode 100644 index 0000000..4da8b5c --- /dev/null +++ b/pykos/setup.py @@ -0,0 +1,45 @@ +# mypy: disable-error-code="import-untyped" +#!/usr/bin/env python +"""Setup script for the project.""" + +import re + +from setuptools import setup + +with open("README.md", "r", encoding="utf-8") as f: + long_description: str = f.read() + + +with open("pykos/requirements.txt", "r", encoding="utf-8") as f: + requirements: list[str] = f.read().splitlines() + + +with open("pykos/requirements-dev.txt", "r", encoding="utf-8") as f: + requirements_dev: list[str] = f.read().splitlines() + + +with open("pykos/__init__.py", "r", encoding="utf-8") as fh: + version_re = re.search(r"^__version__ = \"([^\"]*)\"", fh.read(), re.MULTILINE) +assert version_re is not None, "Could not find version in pykos/__init__.py" +version: str = version_re.group(1) + + +setup( + name="pykos", + version=version, + description="The KOS command line interface", + author="pykos Contributors", + url="https://github.com/kscalelabs/kos", + long_description=long_description, + long_description_content_type="text/markdown", + python_requires=">=3.8", + install_requires=requirements, + tests_require=requirements_dev, + extras_require={"dev": requirements_dev}, + packages=["pykos"], + entry_points={ + "console_scripts": [ + "pykos=pykos.cli:cli", + ], + }, +) \ No newline at end of file diff --git a/setup.py b/setup.py deleted file mode 100644 index 27c388a..0000000 --- a/setup.py +++ /dev/null @@ -1,60 +0,0 @@ -# mypy: disable-error-code="import-untyped" -#!/usr/bin/env python -"""Setup script for the project.""" - -import re -import subprocess - -from setuptools import find_packages, setup -from setuptools.command.build_ext import build_ext -from setuptools_rust import Binding, RustExtension - -with open("README.md", "r", encoding="utf-8") as f: - long_description: str = f.read() - - -with open("kos/requirements.txt", "r", encoding="utf-8") as f: - requirements: list[str] = f.read().splitlines() - - -with open("kos/requirements-dev.txt", "r", encoding="utf-8") as f: - requirements_dev: list[str] = f.read().splitlines() - - -with open("kos/__init__.py", "r", encoding="utf-8") as fh: - version_re = re.search(r"^__version__ = \"([^\"]*)\"", fh.read(), re.MULTILINE) -assert version_re is not None, "Could not find version in kos/__init__.py" -version: str = version_re.group(1) - - -class RustBuildExt(build_ext): - def run(self) -> None: - # Run the stub generator - subprocess.run(["cargo", "run", "--bin", "stub_gen"], check=True) - # Call the original build_ext command - super().run() - - -setup( - name="pykos", - version=version, - description="The K-Scale Operating System", - author="Benjamin Bolte", - url="https://github.com/kscalelabs/kos", - rust_extensions=[ - RustExtension( - target="kos.bindings", - path="kos/bindings/Cargo.toml", - binding=Binding.PyO3, - ), - ], - setup_requires=["setuptools-rust"], - long_description=long_description, - long_description_content_type="text/markdown", - python_requires=">=3.11", - install_requires=requirements, - tests_require=requirements_dev, - extras_require={"dev": requirements_dev}, - packages=find_packages(include=["kos"]), - cmdclass={"build_ext": RustBuildExt}, -) diff --git a/toolchain/Dockerfile b/toolchain/Dockerfile new file mode 100644 index 0000000..135a989 --- /dev/null +++ b/toolchain/Dockerfile @@ -0,0 +1,14 @@ +FROM rust:bookworm + +ENV CROSS_CONTAINER_IN_CONTAINER=true + +RUN apt-get update && apt-get install -y protobuf-compiler gcc-aarch64-linux-gnu g++-aarch64-linux-gnu build-essential \ + libudev-dev +RUN rustup target add aarch64-unknown-linux-gnu && \ + rustup target add x86_64-unknown-linux-gnu && \ + rustup target add aarch64-apple-darwin && \ + rustup target add x86_64-apple-darwin && \ + rustup component add rustfmt clippy + +RUN cargo install cross --git https://github.com/cross-rs/cross +RUN curl -fsSL https://get.docker.com | sh