can_motor_control/
config.rs

1//! TOML config schema, loader, and vendor codec registry.
2
3use std::collections::HashMap;
4use std::path::Path;
5
6use motor_codec::{MotorCodec, MotorTypeId};
7use serde::Deserialize;
8
9use crate::error::Error;
10use crate::robot::{Robot, RobotBuilder};
11use crate::spec::{GripperOpeningSpec, MotorSpec, OpeningDirection};
12use crate::transport::CanBus;
13#[cfg(target_os = "linux")]
14use crate::transport::SocketCanBus;
15#[cfg(target_os = "macos")]
16use crate::transport::{GsUsbBus, GsUsbConfig};
17
18/// Factory function that produces a vendor codec on demand.
19pub type CodecFactory = Box<dyn Fn() -> Box<dyn MotorCodec> + Send + Sync>;
20
21/// Function that resolves a motor type string to a `MotorTypeId` for a given
22/// vendor.
23pub type MotorTypeParser = Box<dyn Fn(&str) -> Option<MotorTypeId> + Send + Sync>;
24
25/// Registry of vendor name → (codec factory, type parser).
26///
27/// `can-motor-control` ships empty by default — populating "damiao" lives in
28/// the `can-motor-damiao-codec` package (or in a higher-level binding crate).
29/// The Python binding (`can-motor-control-py`) registers damiao automatically;
30/// Rust users either build a registry themselves or use the helper exposed by
31/// their vendor codec crate.
32pub struct CodecRegistry {
33    factories: HashMap<String, (CodecFactory, MotorTypeParser)>,
34}
35
36impl Default for CodecRegistry {
37    fn default() -> Self {
38        Self::new()
39    }
40}
41
42impl CodecRegistry {
43    /// Empty registry.
44    pub fn new() -> Self {
45        Self {
46            factories: HashMap::new(),
47        }
48    }
49
50    /// Register a vendor codec + its type-string parser.
51    pub fn register(
52        &mut self,
53        name: impl Into<String>,
54        factory: impl Fn() -> Box<dyn MotorCodec> + Send + Sync + 'static,
55        parser: impl Fn(&str) -> Option<MotorTypeId> + Send + Sync + 'static,
56    ) -> &mut Self {
57        self.factories
58            .insert(name.into(), (Box::new(factory), Box::new(parser)));
59        self
60    }
61
62    fn produce(&self, vendor: &str) -> Option<Box<dyn MotorCodec>> {
63        self.factories.get(vendor).map(|(f, _)| f())
64    }
65
66    fn parse_type(&self, vendor: &str, s: &str) -> Option<MotorTypeId> {
67        self.factories.get(vendor).and_then(|(_, p)| p(s))
68    }
69
70    /// Names of every registered vendor.
71    pub fn vendor_names(&self) -> impl Iterator<Item = &str> {
72        self.factories.keys().map(String::as_str)
73    }
74}
75
76/// Top-level config schema.
77#[derive(Debug, Deserialize)]
78#[serde(deny_unknown_fields)]
79struct RobotConfig {
80    #[serde(default)]
81    bus: HashMap<String, BusConfig>,
82    #[serde(default)]
83    group: Vec<GroupConfig>,
84}
85
86#[derive(Debug, Deserialize)]
87#[serde(deny_unknown_fields)]
88struct BusConfig {
89    kind: String,
90    interface: Option<String>,
91    #[serde(default)]
92    fd: bool,
93    vendor_id: Option<u16>,
94    product_id: Option<u16>,
95    serial_number: Option<String>,
96    index: Option<usize>,
97    bitrate: Option<u32>,
98    initialization_timeout_seconds: Option<f64>,
99    vendor: String,
100}
101
102#[derive(Debug, Deserialize)]
103#[serde(deny_unknown_fields)]
104struct GroupConfig {
105    name: String,
106    kind: String,
107    bus: String,
108    #[serde(default)]
109    default_control_mode: Option<String>,
110    #[serde(default)]
111    opening_direction: Option<String>,
112    #[serde(default)]
113    default_current: Option<f64>,
114    #[serde(default)]
115    motors: Vec<MotorConfig>,
116    motor: Option<MotorConfig>,
117    /// Forbidden: vendor lives on the bus, not on the group. We accept it in
118    /// the schema only so we can produce a helpful error message.
119    vendor: Option<String>,
120}
121
122#[derive(Debug, Deserialize, Clone)]
123#[serde(deny_unknown_fields)]
124struct MotorConfig {
125    name: String,
126    #[serde(rename = "type")]
127    type_str: String,
128    send_id: u32,
129    recv_id: u32,
130}
131
132impl Robot {
133    /// Parse a TOML config file and build a Robot using the supplied codec
134    /// registry. Performs schema validation (including codec-supports-motor)
135    /// before opening any socket. A bus with `fd = true` opens in CAN-FD mode.
136    pub fn from_config<P: AsRef<Path>>(path: P, registry: &CodecRegistry) -> Result<Self, Error> {
137        let text = std::fs::read_to_string(path.as_ref())?;
138        Self::from_config_str(&text, registry)
139    }
140
141    /// Parse a TOML config string. Same semantics as
142    /// [`Robot::from_config`] but skips the file read.
143    pub fn from_config_str(toml_text: &str, registry: &CodecRegistry) -> Result<Self, Error> {
144        let cfg: RobotConfig = toml::from_str(toml_text)
145            .map_err(|e| Error::ConfigSchema(format!("parse error: {e}")))?;
146        // On Linux, `fd = true` opens SocketCAN in CAN-FD mode. The native
147        // macOS gs_usb transport is classical-CAN only.
148        // 1) Detect vendor-on-group with a helpful error.
149        for g in &cfg.group {
150            if let Some(_v) = &g.vendor {
151                return Err(Error::ConfigSchema(format!(
152                    "group '{}': vendor belongs on [bus.<name>], not on [[group]]",
153                    g.name
154                )));
155            }
156            if g.kind != "gripper" && (g.opening_direction.is_some() || g.default_current.is_some())
157            {
158                return Err(Error::ConfigSchema(format!(
159                    "group '{}': opening configuration is only valid for kind='gripper'",
160                    g.name
161                )));
162            }
163        }
164        // 2) Build buses.
165        let mut builder = RobotBuilder::new();
166        // Insertion order in TOML's HashMap is non-deterministic, so we sort
167        // by name for stable behavior.
168        let mut bus_entries: Vec<_> = cfg.bus.into_iter().collect();
169        bus_entries.sort_by(|a, b| a.0.cmp(&b.0));
170        for (bus_name, bus_cfg) in bus_entries {
171            let transport: Box<dyn CanBus> = match bus_cfg.kind.as_str() {
172                #[cfg(target_os = "linux")]
173                "socketcan" => {
174                    let iface = bus_cfg.interface.ok_or_else(|| {
175                        Error::ConfigSchema(format!(
176                            "bus '{bus_name}': socketcan kind requires 'interface' field"
177                        ))
178                    })?;
179                    Box::new(SocketCanBus::open(&iface, bus_cfg.fd)?)
180                }
181                #[cfg(target_os = "macos")]
182                "gs_usb" => {
183                    if bus_cfg.interface.is_some() || bus_cfg.fd {
184                        return Err(Error::ConfigSchema(format!(
185                            "bus '{bus_name}': gs_usb does not accept SocketCAN interface/fd fields"
186                        )));
187                    }
188                    let vendor_id = bus_cfg.vendor_id.ok_or_else(|| {
189                        Error::ConfigSchema(format!(
190                            "bus '{bus_name}': gs_usb requires 'vendor_id'"
191                        ))
192                    })?;
193                    let product_id = bus_cfg.product_id.ok_or_else(|| {
194                        Error::ConfigSchema(format!(
195                            "bus '{bus_name}': gs_usb requires 'product_id'"
196                        ))
197                    })?;
198                    let mut config = GsUsbConfig::new(vendor_id, product_id);
199                    config.serial_number = bus_cfg.serial_number;
200                    config.index = bus_cfg.index;
201                    if let Some(bitrate) = bus_cfg.bitrate {
202                        config.bitrate = bitrate;
203                    }
204                    if let Some(seconds) = bus_cfg.initialization_timeout_seconds {
205                        if !seconds.is_finite() || seconds <= 0.0 {
206                            return Err(Error::ConfigSchema(format!(
207                                "bus '{bus_name}': initialization_timeout_seconds must be finite and > 0"
208                            )));
209                        }
210                        config.initialization_timeout = std::time::Duration::from_secs_f64(seconds);
211                    }
212                    Box::new(GsUsbBus::open(config)?)
213                }
214                other => {
215                    return Err(Error::ConfigSchema(format!(
216                        "bus '{bus_name}': unsupported transport kind '{other}' on {}",
217                        std::env::consts::OS
218                    )));
219                }
220            };
221            let codec = registry
222                .produce(&bus_cfg.vendor)
223                .ok_or_else(|| Error::UnknownVendor(bus_cfg.vendor.clone()))?;
224            builder = builder.add_bus(bus_name, transport, codec);
225        }
226        // 3) Build groups; resolve motor type strings via the bus's vendor.
227        // We need the bus's vendor for type resolution — look it up by walking
228        // the original cfg.bus, which we no longer have. Workaround: rebuild
229        // a name → vendor map from the second copy below. To keep things
230        // simple we re-parse to a local lookup.
231        let cfg2: RobotConfig = toml::from_str(toml_text)
232            .map_err(|e| Error::ConfigSchema(format!("re-parse error: {e}")))?;
233        let bus_vendor: HashMap<String, String> = cfg2
234            .bus
235            .iter()
236            .map(|(n, b)| (n.clone(), b.vendor.clone()))
237            .collect();
238        for g in cfg2.group {
239            let vendor = bus_vendor
240                .get(&g.bus)
241                .ok_or_else(|| Error::UnknownBusName(g.bus.clone()))?;
242            let resolve = |m: &MotorConfig| -> Result<MotorSpec, Error> {
243                let mt = registry.parse_type(vendor, &m.type_str).ok_or_else(|| {
244                    Error::ConfigSchema(format!(
245                        "group '{}': vendor '{}' does not recognize motor type '{}'",
246                        g.name, vendor, m.type_str
247                    ))
248                })?;
249                Ok(MotorSpec::new(m.name.clone(), mt, m.send_id, m.recv_id))
250            };
251            match g.kind.as_str() {
252                "arm" => {
253                    let motors: Result<Vec<_>, _> = g.motors.iter().map(resolve).collect();
254                    builder = builder.add_arm(g.name, g.bus, motors?);
255                }
256                "gripper" => {
257                    let opening = parse_gripper_opening(&g)?;
258                    let motor_cfg = g.motor.ok_or_else(|| {
259                        Error::ConfigSchema(format!(
260                            "group '{}': kind='gripper' requires 'motor' (singular), not 'motors'",
261                            g.name
262                        ))
263                    })?;
264                    let m = resolve(&motor_cfg)?;
265                    builder = match opening {
266                        Some(opening) => {
267                            builder.add_gripper_with_opening(g.name, g.bus, m, opening)
268                        }
269                        None => builder.add_gripper(g.name, g.bus, m),
270                    };
271                }
272                "generic" => {
273                    let motors: Result<Vec<_>, _> = g.motors.iter().map(resolve).collect();
274                    builder = builder.add_generic(g.name, g.bus, motors?);
275                }
276                other => {
277                    return Err(Error::ConfigSchema(format!(
278                        "group '{}': unknown kind '{other}' (expected arm|gripper|generic)",
279                        g.name
280                    )))
281                }
282            }
283        }
284        builder.build()
285    }
286}
287
288fn parse_gripper_opening(g: &GroupConfig) -> Result<Option<GripperOpeningSpec>, Error> {
289    let direction = match g.opening_direction.as_deref() {
290        Some("increasing_position") => Some(OpeningDirection::IncreasingPosition),
291        Some("decreasing_position") => Some(OpeningDirection::DecreasingPosition),
292        Some(other) => {
293            return Err(Error::ConfigSchema(format!(
294                "group '{}': unknown opening_direction '{other}' (expected increasing_position|decreasing_position)",
295                g.name
296            )));
297        }
298        None => None,
299    };
300    if let Some(current) = g.default_current {
301        if current <= 0.0 || current > 1.0 {
302            return Err(Error::ConfigSchema(format!(
303                "group '{}': default_current must be > 0.0 and <= 1.0, got {current}",
304                g.name
305            )));
306        }
307        if direction.is_none() {
308            return Err(Error::ConfigSchema(format!(
309                "group '{}': default_current requires opening_direction",
310                g.name
311            )));
312        }
313    }
314    Ok(direction.map(|direction| GripperOpeningSpec::new(direction, g.default_current)))
315}
316
317#[cfg(test)]
318mod tests {
319    use super::*;
320
321    fn gripper_config(
322        opening_direction: Option<&str>,
323        default_current: Option<f64>,
324    ) -> GroupConfig {
325        GroupConfig {
326            name: "grip".to_string(),
327            kind: "gripper".to_string(),
328            bus: "main".to_string(),
329            default_control_mode: None,
330            opening_direction: opening_direction.map(str::to_string),
331            default_current,
332            motors: Vec::new(),
333            motor: None,
334            vendor: None,
335        }
336    }
337
338    #[test]
339    fn parse_opening_direction_increasing() {
340        let cfg = gripper_config(Some("increasing_position"), Some(0.2));
341        let got = parse_gripper_opening(&cfg).unwrap().unwrap();
342        assert_eq!(got.direction, OpeningDirection::IncreasingPosition);
343        assert_eq!(got.default_current, Some(0.2));
344    }
345
346    #[test]
347    fn parse_opening_direction_decreasing() {
348        let cfg = gripper_config(Some("decreasing_position"), None);
349        let got = parse_gripper_opening(&cfg).unwrap().unwrap();
350        assert_eq!(got.direction, OpeningDirection::DecreasingPosition);
351        assert_eq!(got.default_current, None);
352    }
353
354    #[test]
355    fn parse_opening_rejects_unknown_direction() {
356        let cfg = gripper_config(Some("clockwise"), None);
357        assert!(matches!(
358            parse_gripper_opening(&cfg),
359            Err(Error::ConfigSchema(_))
360        ));
361    }
362
363    #[test]
364    fn parse_opening_rejects_invalid_default_current() {
365        let cfg = gripper_config(Some("increasing_position"), Some(1.5));
366        assert!(matches!(
367            parse_gripper_opening(&cfg),
368            Err(Error::ConfigSchema(_))
369        ));
370    }
371
372    #[test]
373    fn parse_opening_requires_direction_for_default_current() {
374        let cfg = gripper_config(None, Some(0.2));
375        assert!(matches!(
376            parse_gripper_opening(&cfg),
377            Err(Error::ConfigSchema(_))
378        ));
379    }
380}
can_motor_control/config.rs

can_motor_control/
config.rs
