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

use arci::BaseVelocity;

use crate::msg::geometry_msgs::{Twist, Vector3};

impl From<BaseVelocity> for Twist {
    fn from(base_velocity: BaseVelocity) -> Self {
        Self {
            linear: Vector3 {
                x: base_velocity.x,
                y: base_velocity.y,
                z: 0.0,
            },
            angular: Vector3 {
                x: 0.0,
                y: 0.0,
                z: base_velocity.theta,
            },
        }
    }
}

#[allow(clippy::from_over_into)]
impl Into<BaseVelocity> for Twist {
    fn into(self) -> BaseVelocity {
        BaseVelocity {
            x: self.linear.x,
            y: self.linear.y,
            theta: self.angular.z,
        }
    }
}

#[cfg(test)]
mod tests {
    use arci::BaseVelocity;
    use assert_approx_eq::assert_approx_eq;

    use super::{Twist, Vector3};

    #[test]
    fn test_base_velocity_to_twist() {
        let base_velocity = BaseVelocity {
            x: 1.0,
            y: 2.0,
            theta: 3.0,
        };
        let twist = Twist::from(base_velocity);
        assert_approx_eq!(twist.linear.x, 1.0);
        assert_approx_eq!(twist.linear.y, 2.0);
        assert_approx_eq!(twist.linear.z, 0.0);
        assert_approx_eq!(twist.angular.x, 0.0);
        assert_approx_eq!(twist.angular.y, 0.0);
        assert_approx_eq!(twist.angular.z, 3.0);
    }

    #[test]
    fn test_twist_to_base_velocity() {
        let twist = Twist {
            linear: Vector3 {
                x: 1.0,
                y: 2.0,
                z: 3.0,
            },
            angular: Vector3 {
                x: 4.0,
                y: 5.0,
                z: 6.0,
            },
        };
        let base_velocity: BaseVelocity = twist.into();
        assert_approx_eq!(base_velocity.x, 1.0);
        assert_approx_eq!(base_velocity.y, 2.0);
        assert_approx_eq!(base_velocity.theta, 6.0);
    }
}