supervisor running as background thread in rust (#15)

* supervisor running as background thread in rust

* Bump version to 0.0.10

actuator/__init__.py

@@ -1,3 +1,7 @@
-__version__ = "0.0.9"
+__version__ = "0.0.10"
 
-from .rust.bindings import PyRobstrideMotorFeedback as RobstrideMotorFeedback, PyRobstrideMotors as RobstrideMotors
+from .rust.bindings import (
+    PyRobstrideMotorFeedback as RobstrideMotorFeedback,
+    PyRobstrideMotors as RobstrideMotors,
+    PyRobstrideMotorsSupervisor as RobstrideMotorsSupervisor,
+)

actuator/rust/bindings/bindings.pyi

@@ -41,3 +41,27 @@ class PyRobstrideMotors:
         ...
 
 
+class PyRobstrideMotorsSupervisor:
+    def __new__(cls,port_name:str, motor_infos:typing.Mapping[int, str]): ...
+    def set_target_position(self, motor_id:int, position:float) -> None:
+        ...
+
+    def set_kp_kd(self, motor_id:int, kp:float, kd:float) -> None:
+        ...
+
+    def set_sleep_duration(self, sleep_duration:float) -> None:
+        ...
+
+    def add_motor_to_zero(self, motor_id:int) -> None:
+        ...
+
+    def get_latest_feedback(self) -> dict[int, PyRobstrideMotorFeedback]:
+        ...
+
+    def stop(self) -> None:
+        ...
+
+    def __repr__(self) -> str:
+        ...
+
+

actuator/rust/bindings/src/lib.rs

@@ -2,11 +2,12 @@ use pyo3::prelude::*;
 use pyo3_stub_gen::define_stub_info_gatherer;
 use pyo3_stub_gen::derive::{gen_stub_pyclass, gen_stub_pymethods};
 use robstride::{
-    MotorFeedback as RobstrideMotorFeedback, MotorType as RobstrideMotorType,
-    Motors as RobstrideMotors,
+    motor_type_from_str as robstride_motor_type_from_str, MotorFeedback as RobstrideMotorFeedback,
+    MotorType as RobstrideMotorType, Motors as RobstrideMotors,
+    MotorsSupervisor as RobstrideMotorsSupervisor,
 };
 use std::collections::HashMap;
-
+use std::time::Duration;
 #[gen_stub_pyclass]
 #[pyclass]
 struct PyRobstrideMotors {
@@ -21,22 +22,12 @@ impl PyRobstrideMotors {
         let motor_infos = motor_infos
             .into_iter()
             .map(|(id, type_str)| {
-                let motor_type = match type_str.as_str() {
-                    "01" => RobstrideMotorType::Type01,
-                    "03" => RobstrideMotorType::Type03,
-                    "04" => RobstrideMotorType::Type04,
-                    _ => {
-                        return Err(PyErr::new::<pyo3::exceptions::PyValueError, _>(format!(
-                            "Invalid motor type: {}",
-                            type_str
-                        )))
-                    }
-                };
+                let motor_type = robstride_motor_type_from_str(type_str.as_str())?;
                 Ok((id, motor_type))
             })
             .collect::<PyResult<HashMap<u8, RobstrideMotorType>>>()?;
 
-        let motors = RobstrideMotors::new(&port_name, motor_infos)
+        let motors = RobstrideMotors::new(&port_name, &motor_infos)
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
 
         Ok(PyRobstrideMotors { inner: motors })
@@ -156,10 +147,79 @@ impl From<RobstrideMotorFeedback> for PyRobstrideMotorFeedback {
     }
 }
 
+#[gen_stub_pyclass]
+#[pyclass]
+struct PyRobstrideMotorsSupervisor {
+    inner: RobstrideMotorsSupervisor,
+}
+
+#[gen_stub_pymethods]
+#[pymethods]
+impl PyRobstrideMotorsSupervisor {
+    #[new]
+    fn new(port_name: String, motor_infos: HashMap<u8, String>) -> PyResult<Self> {
+        let motor_infos = motor_infos
+            .into_iter()
+            .map(|(id, type_str)| {
+                let motor_type = robstride_motor_type_from_str(type_str.as_str())?;
+                Ok((id, motor_type))
+            })
+            .collect::<PyResult<HashMap<u8, RobstrideMotorType>>>()?;
+
+        let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+
+        Ok(PyRobstrideMotorsSupervisor { inner: controller })
+    }
+
+    fn set_target_position(&self, motor_id: u8, position: f32) -> PyResult<()> {
+        self.inner.set_target_position(motor_id, position);
+        Ok(())
+    }
+
+    fn set_kp_kd(&self, motor_id: u8, kp: f32, kd: f32) -> PyResult<()> {
+        self.inner.set_kp_kd(motor_id, kp, kd);
+        Ok(())
+    }
+
+    fn set_sleep_duration(&self, sleep_duration: f32) -> PyResult<()> {
+        self.inner
+            .set_sleep_duration(Duration::from_millis(sleep_duration as u64));
+        Ok(())
+    }
+
+    fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> {
+        self.inner.add_motor_to_zero(motor_id);
+        Ok(())
+    }
+
+    fn get_latest_feedback(&self) -> HashMap<u8, PyRobstrideMotorFeedback> {
+        self.inner
+            .get_latest_feedback()
+            .into_iter()
+            .map(|(k, v)| (k, v.into()))
+            .collect()
+    }
+
+    fn stop(&self) -> PyResult<()> {
+        self.inner.stop();
+        Ok(())
+    }
+
+    fn __repr__(&self) -> PyResult<String> {
+        let motor_count = self.inner.get_latest_feedback().len();
+        Ok(format!(
+            "PyRobstrideMotorsSupervisor(motor_count={})",
+            motor_count
+        ))
+    }
+}
+
 #[pymodule]
 fn bindings(m: &Bound<PyModule>) -> PyResult<()> {
     m.add_class::<PyRobstrideMotors>()?;
     m.add_class::<PyRobstrideMotorFeedback>()?;
+    m.add_class::<PyRobstrideMotorsSupervisor>()?;
     Ok(())
 }
 

actuator/rust/robstride/src/main.rs → actuator/rust/robstride/src/bin/motors.rs

@@ -1,15 +1,14 @@
-use robstride::{MotorType, Motors};
+use robstride::{motor_type_from_str, Motors};
 use std::collections::HashMap;
 use std::error::Error;
 use std::f32::consts::PI;
 use std::io::{self, Write};
 use std::time::Instant;
 
-const TEST_ID: u8 = 2;
 const RUN_TIME: f32 = 3.0;
 const MAX_TORQUE: f32 = 1.0;
 
-fn run_motion_test(motors: &mut Motors) -> Result<(), Box<dyn Error>> {
+fn run_motion_test(motors: &mut Motors, test_id: u8) -> Result<(), Box<dyn Error>> {
     motors.send_reset()?;
     motors.send_start()?;
 
@@ -29,26 +28,26 @@ fn run_motion_test(motors: &mut Motors) -> Result<(), Box<dyn Error>> {
         let elapsed_time = start_time.elapsed().as_secs_f32();
         let desired_position = amplitude * (elapsed_time * PI * 2.0 / period + PI / 2.0).cos();
 
-        let feedback = motors.get_latest_feedback_for(TEST_ID)?.clone();
+        let feedback = motors.get_latest_feedback_for(test_id)?.clone();
         let current_position = feedback.position;
         let current_velocity = feedback.velocity;
         let torque = (kp_04 * (desired_position - current_position) - kd_04 * current_velocity)
             .clamp(-MAX_TORQUE, MAX_TORQUE);
 
-        motors.send_torque_controls(&HashMap::from([(TEST_ID, torque)]))?;
+        motors.send_torque_controls(&HashMap::from([(test_id, torque)]))?;
 
         command_count += 1;
         println!(
             "Motor {} Commands: {}, Frequency: {:.2} Hz, Desired position: {:.2} Feedback: {:?}",
-            TEST_ID,
+            test_id,
             command_count,
             command_count as f32 / elapsed_time,
             desired_position,
             feedback
         );
     }
 
-    motors.send_torque_controls(&HashMap::from([(TEST_ID, 0.0)]))?;
+    motors.send_torque_controls(&HashMap::from([(test_id, 0.0)]))?;
     motors.send_reset()?;
 
     let elapsed_time = start_time.elapsed().as_secs_f32();
@@ -66,11 +65,37 @@ fn print_current_mode(motors: &mut Motors) {
 }
 
 fn main() -> Result<(), Box<dyn Error>> {
+    print!("Enter the TEST_ID (u8): ");
+    io::stdout().flush()?;
+    let mut input = String::new();
+    io::stdin().read_line(&mut input)?;
+    let test_id: u8 = input.trim().parse()?;
+
+    print!("Enter the port name (default: /dev/ttyUSB0): ");
+    io::stdout().flush()?;
+    let mut port_input = String::new();
+    io::stdin().read_line(&mut port_input)?;
+    let port_name = port_input.trim().to_string();
+    let port_name = if port_name.is_empty() {
+        String::from("/dev/ttyUSB0")
+    } else {
+        port_name
+    };
+
+    print!("Enter the motor type (default: 01): ");
+    io::stdout().flush()?;
+    let mut motor_type_input = String::new();
+    io::stdin().read_line(&mut motor_type_input)?;
+    let motor_type_input = motor_type_input.trim().to_string();
+    let motor_type_input = if motor_type_input.is_empty() {
+        String::from("01")
+    } else {
+        motor_type_input
+    };
+    let motor_type = motor_type_from_str(motor_type_input.as_str())?;
+
     // Create motor instances
-    let mut motors = Motors::new(
-        "/dev/ttyUSB0",
-        HashMap::from([(TEST_ID, MotorType::Type01)]),
-    )?;
+    let mut motors = Motors::new(&port_name, HashMap::from([(test_id, motor_type)]))?;
 
     let mut last_command: i32 = -1;
 
@@ -98,7 +123,7 @@ fn main() -> Result<(), Box<dyn Error>> {
                 last_command = 1;
             }
             2 => {
-                run_motion_test(&mut motors)?;
+                run_motion_test(&mut motors, test_id)?;
                 last_command = 2;
             }
             3 => break,

actuator/rust/robstride/src/bin/supervisor.rs

@@ -0,0 +1,99 @@
+use robstride::{motor_type_from_str, MotorType, MotorsSupervisor};
+use std::collections::HashMap;
+use std::io::{self, Write};
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    print!("Enter the TEST_ID (u8): ");
+    io::stdout().flush()?;
+    let mut input = String::new();
+    io::stdin().read_line(&mut input)?;
+    let test_id: u8 = input.trim().parse()?;
+
+    print!("Enter the port name (default: /dev/ttyUSB0): ");
+    io::stdout().flush()?;
+    let mut port_input = String::new();
+    io::stdin().read_line(&mut port_input)?;
+    let port_name = port_input.trim().to_string();
+    let port_name = if port_name.is_empty() {
+        String::from("/dev/ttyUSB0")
+    } else {
+        port_name
+    };
+
+    print!("Enter the motor type (default: 01): ");
+    io::stdout().flush()?;
+    let mut motor_type_input = String::new();
+    io::stdin().read_line(&mut motor_type_input)?;
+    let motor_type_input = motor_type_input.trim().to_string();
+    let motor_type_input = if motor_type_input.is_empty() {
+        String::from("01")
+    } else {
+        motor_type_input
+    };
+    let motor_type = motor_type_from_str(motor_type_input.as_str())?;
+    let motor_infos: HashMap<u8, MotorType> = HashMap::from([(test_id, motor_type)]);
+    let controller = MotorsSupervisor::new(&port_name, &motor_infos)?;
+
+    println!("Motor Controller Test CLI");
+    println!("Available commands:");
+    println!("  set_position / s <position>");
+    println!("  set_kp_kd / k <kp> <kd>");
+    println!("  zero / z");
+    println!("  get_feedback / g");
+    println!("  quit / q");
+
+    loop {
+        print!("> ");
+        io::stdout().flush()?;
+
+        let mut input = String::new();
+        io::stdin().read_line(&mut input)?;
+        let parts: Vec<&str> = input.trim().split_whitespace().collect();
+
+        if parts.is_empty() {
+            continue;
+        }
+
+        match parts[0] {
+            "set_position" | "s" => {
+                if parts.len() != 2 {
+                    println!("Usage: set_position <position>");
+                    continue;
+                }
+                let position: f32 = parts[1].parse()?;
+                controller.set_target_position(test_id, position);
+                println!("Set target position to {}", position);
+            }
+            "set_kp_kd" | "k" => {
+                if parts.len() != 3 {
+                    println!("Usage: set_kp_kd <kp> <kd>");
+                    continue;
+                }
+                let kp: f32 = parts[1].parse()?;
+                let kd: f32 = parts[2].parse()?;
+                controller.set_kp_kd(test_id, kp, kd);
+                println!("Set KP/KD for motor {} to {}/{}", test_id, kp, kd);
+            }
+            "zero" | "z" => {
+                controller.add_motor_to_zero(test_id);
+                println!("Added motor {} to zero list", test_id);
+            }
+            "get_feedback" | "g" => {
+                let feedback = controller.get_latest_feedback();
+                for (id, fb) in feedback {
+                    println!("Motor {}: {:?}", id, fb);
+                }
+            }
+            "quit" | "q" => {
+                controller.stop();
+                println!("Exiting...");
+                break;
+            }
+            _ => {
+                println!("Unknown command. Available commands: set_position, set_kp_kd, get_feedback, quit");
+            }
+        }
+    }
+
+    Ok(())
+}