1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139

use anyhow::Result;
use nalgebra::Vector3;
use serde::{Deserialize, Serialize};
use std::{fs, include_bytes, str};

#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)]
pub struct ArmConfig {
    pub base_id: u8,
    pub shoulder_id: u8,
    pub elbow_id: u8,
    pub wrist_id: u8,
    pub gripper_id: u8,
    pub shoulder: Vector3<f32>,
    pub elbow: Vector3<f32>,
    pub wrist: Vector3<f32>,
    pub end_effector: Vector3<f32>,
}

impl Default for ArmConfig {
    /// Arm configuration with default values
    fn default() -> ArmConfig {
        ArmConfig {
            base_id: 1,
            shoulder_id: 2,
            elbow_id: 3,
            wrist_id: 4,
            gripper_id: 5,
            shoulder: Vector3::new(0.0, 0.0, 0.0),
            elbow: Vector3::new(0.0, 0.0, 0.0),
            wrist: Vector3::new(0.0, 0.0, 0.0),
            end_effector: Vector3::new(0.0, 0.0, 0.0),
        }
    }
}

impl ArmConfig {
    pub fn get_ids(&self) -> [u8; 5] {
        [
            self.base_id,
            self.shoulder_id,
            self.elbow_id,
            self.wrist_id,
            self.gripper_id,
        ]
    }

    /// Guppy comes with an included config file.
    ///
    /// This file is packaged with the binary
    /// This method retrieves this included version
    pub fn included() -> ArmConfig {
        let json = str::from_utf8(include_bytes!("../config/guppy.json")).unwrap();
        ArmConfig::parse_json(json).unwrap()
    }

    pub fn parse_json(text: &str) -> Result<ArmConfig> {
        let config: ArmConfig = serde_json::from_str(text)?;
        Ok(config)
    }

    pub fn parse_yaml(text: &str) -> Result<ArmConfig> {
        let config: ArmConfig = serde_yaml::from_str(text)?;
        Ok(config)
    }

    pub fn serialize_to_json(&self) -> Result<String> {
        let json = serde_json::to_string_pretty(self)?;
        Ok(json)
    }

    pub fn serialize_to_yaml(&self) -> Result<String> {
        let yaml = serde_yaml::to_string(self)?;
        Ok(yaml)
    }

    pub fn save_json(&self, path: &str) -> Result<()> {
        fs::write(path, self.serialize_to_json()?)?;
        Ok(())
    }

    pub fn save_yaml(&self, path: &str) -> Result<()> {
        fs::write(path, self.serialize_to_yaml()?)?;
        Ok(())
    }

    pub fn load_json(path: &str) -> Result<ArmConfig> {
        let text = fs::read_to_string(path)?;
        let config = ArmConfig::parse_json(&text)?;
        Ok(config)
    }

    pub fn load_yaml(path: &str) -> Result<ArmConfig> {
        let text = fs::read_to_string(path)?;
        let config = ArmConfig::parse_yaml(&text)?;
        Ok(config)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    const DEFAULT_JSON: &str = "{\"base_id\":1,\"shoulder_id\":2,\"elbow_id\":3,\"wrist_id\":4,\"gripper_id\":5,\"shoulder\":[0,0,0.0],\"elbow\":[0.0,0,0.0],\"wrist\":[0.0,0,0],\"end_effector\":[0,0,0.0]}";

    #[test]
    fn parse_from_json() {
        let json = DEFAULT_JSON;
        let config = ArmConfig::parse_json(json).unwrap();
        assert_eq!(config, ArmConfig::default());
    }

    #[test]
    fn parse_from_yaml() {
        let yaml = DEFAULT_JSON;
        let config = ArmConfig::parse_yaml(yaml).unwrap();
        assert_eq!(config, ArmConfig::default());
    }

    #[test]
    fn serialize_to_json() {
        let config = ArmConfig::default();
        let json = config.serialize_to_json().unwrap();
        let parsed_config = ArmConfig::parse_json(&json).unwrap();
        assert_eq!(config, parsed_config);
    }

    #[test]
    fn serialize_to_yaml() {
        let config = ArmConfig::default();
        let yaml = config.serialize_to_yaml().unwrap();
        let parsed_config = ArmConfig::parse_yaml(&yaml).unwrap();
        assert_eq!(config, parsed_config);
    }

    #[test]
    fn check_included() {
        let _ = ArmConfig::included();
    }
}