diff --git a/Cargo.lock b/Cargo.lock index fa4f7c5b..f2995c84 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3393,6 +3393,7 @@ version = "0.1.0" dependencies = [ "dora-node-api", "eyre", + "pyo3", "rustypot", "serialport", ] diff --git a/examples/so100-remote/no_torque.yml b/examples/so100-remote/no_torque.yml index e8af5218..570e9b2e 100644 --- a/examples/so100-remote/no_torque.yml +++ b/examples/so100-remote/no_torque.yml @@ -1,6 +1,7 @@ nodes: - id: so100 path: dora-rustypot + build: pip install -e ../../node-hub/dora-rustypot inputs: tick: dora/timer/millis/33 #pose: pytorch-kinematics/action diff --git a/node-hub/dora-rustypot/Cargo.toml b/node-hub/dora-rustypot/Cargo.toml index 5f0f24e8..4a0832bd 100644 --- a/node-hub/dora-rustypot/Cargo.toml +++ b/node-hub/dora-rustypot/Cargo.toml @@ -10,3 +10,19 @@ dora-node-api = { workspace = true } eyre = "0.6.12" rustypot = { version = "1.0" } serialport = { version = "4.7.1", default-features = false } +pyo3 = { workspace = true, features = [ + "extension-module", + "abi3", + "eyre", + "generate-import-lib", +], optional = true } + + +[features] +default = [] +python = ["pyo3"] + +[lib] +name = "dora_rustypot" +path = "src/lib.rs" +crate-type = ["lib", "cdylib"] diff --git a/node-hub/dora-rustypot/pyproject.toml b/node-hub/dora-rustypot/pyproject.toml new file mode 100644 index 00000000..1246a198 --- /dev/null +++ b/node-hub/dora-rustypot/pyproject.toml @@ -0,0 +1,28 @@ +[build-system] +requires = ["maturin>=0.13.2"] +build-backend = "maturin" + +[project] +name = "dora-rustypot" +dynamic = ["version"] +license = { text = "MIT" } +requires-python = ">=3.10" + +dependencies = [] + +scripts = { "dora-rustypot" = "dora_rustypot:py_main" } + +[tool.maturin] +features = ["python", "pyo3/extension-module"] + +[tool.ruff.lint] +extend-select = [ + "D", # pydocstyle + "UP", # Ruff's UP rule + "PERF", # Ruff's PERF rule + "RET", # Ruff's RET rule + "RSE", # Ruff's RSE rule + "NPY", # Ruff's NPY rule + "N", # Ruff's N rule + "I", # Ruff's I rule +] diff --git a/node-hub/dora-rustypot/src/lib.rs b/node-hub/dora-rustypot/src/lib.rs new file mode 100644 index 00000000..ed98bde8 --- /dev/null +++ b/node-hub/dora-rustypot/src/lib.rs @@ -0,0 +1,101 @@ +use dora_node_api::dora_core::config::DataId; +use dora_node_api::{into_vec, DoraNode, Event, IntoArrow, Parameter}; +use eyre::{Context, Result}; +use rustypot::servo::feetech::sts3215::Sts3215Controller; +use std::collections::BTreeMap; +use std::time::Duration; + +pub fn lib_main() -> Result<()> { + let (mut node, mut events) = DoraNode::init_from_env()?; + let serialportname: String = std::env::var("PORT").context("Serial port name not provided")?; + let baudrate: u32 = std::env::var("BAUDRATE") + .unwrap_or_else(|_| "1000000".to_string()) + .parse() + .context("Invalid baudrate")?; + let ids = std::env::var("IDS") + .unwrap_or_else(|_| "1,2,3,4,5,6".to_string()) + .split(&[',', ' '][..]) + .map(|s| s.parse::().unwrap()) + .collect::>(); + + let serial_port = serialport::new(serialportname, baudrate) + .timeout(Duration::from_millis(1000)) + .open()?; + + let mut c = Sts3215Controller::new() + .with_protocol_v1() + .with_serial_port(serial_port); + + if let Ok(torque) = std::env::var("TORQUE") { + let truthies = vec![true; ids.len()]; + + c.write_torque_enable(&ids, &truthies) + .expect("could not enable torque"); + + if let Ok(torque_limit) = torque.parse::() { + let limits = vec![torque_limit; ids.len()]; + c.write_torque_limit(&ids, &limits) + .expect("could not enable torque"); + } + } else { + let falsies = vec![false; ids.len()]; + c.write_torque_enable(&ids, &falsies) + .expect("could not enable torque"); + } + + while let Some(event) = events.recv() { + match event { + Event::Input { + id, + metadata: _, + data, + } => match id.as_str() { + "tick" => { + if let Ok(joints) = c.read_present_position(&ids) { + let mut parameter = BTreeMap::new(); + parameter.insert( + "encoding".to_string(), + Parameter::String("jointstate".to_string()), + ); + node.send_output( + DataId::from("pose".to_string()), + parameter, + joints.into_arrow(), + ) + .unwrap(); + }; + } + "pose" => { + let data: Vec = into_vec(&data).expect("could not cast values"); + c.write_goal_position(&ids, &data).unwrap(); + } + other => eprintln!("Received input `{other}`"), + }, + _ => {} + } + } + + Ok(()) +} + +#[cfg(feature = "python")] +use pyo3::{ + pyfunction, pymodule, + types::{PyModule, PyModuleMethods}, + wrap_pyfunction, Bound, PyResult, Python, +}; + +#[cfg(feature = "python")] +#[pyfunction] +fn py_main(_py: Python) -> eyre::Result<()> { + lib_main() +} + +/// A Python module implemented in Rust. +#[cfg(feature = "python")] +#[pymodule] +fn dora_rustypot(_py: Python, m: Bound<'_, PyModule>) -> PyResult<()> { + m.add_function(wrap_pyfunction!(py_main, &m)?)?; + m.add("__version__", env!("CARGO_PKG_VERSION"))?; + Ok(()) +} diff --git a/node-hub/dora-rustypot/src/main.rs b/node-hub/dora-rustypot/src/main.rs index 650e263c..1f8949be 100644 --- a/node-hub/dora-rustypot/src/main.rs +++ b/node-hub/dora-rustypot/src/main.rs @@ -1,79 +1,3 @@ -use dora_node_api::dora_core::config::DataId; -use dora_node_api::{into_vec, DoraNode, Event, IntoArrow, Parameter}; -use eyre::{Context, Result}; -use rustypot::servo::feetech::sts3215::Sts3215Controller; -use std::collections::BTreeMap; -use std::time::Duration; - -fn main() -> Result<()> { - let (mut node, mut events) = DoraNode::init_from_env()?; - let serialportname: String = std::env::var("PORT").context("Serial port name not provided")?; - let baudrate: u32 = std::env::var("BAUDRATE") - .unwrap_or_else(|_| "1000000".to_string()) - .parse() - .context("Invalid baudrate")?; - let ids = std::env::var("IDS") - .unwrap_or_else(|_| "1,2,3,4,5,6".to_string()) - .split(&[',', ' '][..]) - .map(|s| s.parse::().unwrap()) - .collect::>(); - - let serial_port = serialport::new(serialportname, baudrate) - .timeout(Duration::from_millis(1000)) - .open()?; - - let mut c = Sts3215Controller::new() - .with_protocol_v1() - .with_serial_port(serial_port); - - if let Ok(torque) = std::env::var("TORQUE") { - let truthies = vec![true; ids.len()]; - - c.write_torque_enable(&ids, &truthies) - .expect("could not enable torque"); - - if let Ok(torque_limit) = torque.parse::() { - let limits = vec![torque_limit; ids.len()]; - c.write_torque_limit(&ids, &limits) - .expect("could not enable torque"); - } - } else { - let falsies = vec![false; ids.len()]; - c.write_torque_enable(&ids, &falsies) - .expect("could not enable torque"); - } - - while let Some(event) = events.recv() { - match event { - Event::Input { - id, - metadata: _, - data, - } => match id.as_str() { - "tick" => { - if let Ok(joints) = c.read_present_position(&ids) { - let mut parameter = BTreeMap::new(); - parameter.insert( - "encoding".to_string(), - Parameter::String("jointstate".to_string()), - ); - node.send_output( - DataId::from("pose".to_string()), - parameter, - joints.into_arrow(), - ) - .unwrap(); - }; - } - "pose" => { - let data: Vec = into_vec(&data).expect("could not cast values"); - c.write_goal_position(&ids, &data).unwrap(); - } - other => eprintln!("Received input `{other}`"), - }, - _ => {} - } - } - - Ok(()) +fn main() -> Result<(), eyre::Error> { + dora_rustypot::lib_main() }