arci_ros/
msg_utils.rs

Help
1use arci::BaseVelocity;
2
3use crate::msg::geometry_msgs::{Twist, Vector3};
4
5impl From<BaseVelocity> for Twist {
6    fn from(base_velocity: BaseVelocity) -> Self {
7        Self {
8            linear: Vector3 {
9                x: base_velocity.x,
10                y: base_velocity.y,
11                z: 0.0,
12            },
13            angular: Vector3 {
14                x: 0.0,
15                y: 0.0,
16                z: base_velocity.theta,
17            },
18        }
19    }
20}
21
22#[allow(clippy::from_over_into)]
23impl Into<BaseVelocity> for Twist {
24    fn into(self) -> BaseVelocity {
25        BaseVelocity {
26            x: self.linear.x,
27            y: self.linear.y,
28            theta: self.angular.z,
29        }
30    }
31}
32
33#[cfg(test)]
34mod tests {
35    use arci::BaseVelocity;
36    use assert_approx_eq::assert_approx_eq;
37
38    use super::{Twist, Vector3};
39
40    #[test]
41    fn test_base_velocity_to_twist() {
42        let base_velocity = BaseVelocity {
43            x: 1.0,
44            y: 2.0,
45            theta: 3.0,
46        };
47        let twist = Twist::from(base_velocity);
48        assert_approx_eq!(twist.linear.x, 1.0);
49        assert_approx_eq!(twist.linear.y, 2.0);
50        assert_approx_eq!(twist.linear.z, 0.0);
51        assert_approx_eq!(twist.angular.x, 0.0);
52        assert_approx_eq!(twist.angular.y, 0.0);
53        assert_approx_eq!(twist.angular.z, 3.0);
54    }
55
56    #[test]
57    fn test_twist_to_base_velocity() {
58        let twist = Twist {
59            linear: Vector3 {
60                x: 1.0,
61                y: 2.0,
62                z: 3.0,
63            },
64            angular: Vector3 {
65                x: 4.0,
66                y: 5.0,
67                z: 6.0,
68            },
69        };
70        let base_velocity: BaseVelocity = twist.into();
71        assert_approx_eq!(base_velocity.x, 1.0);
72        assert_approx_eq!(base_velocity.y, 2.0);
73        assert_approx_eq!(base_velocity.theta, 6.0);
74    }
75}
arci_ros/msg_utils.rs

arci_ros/
msg_utils.rs
