transforms_io 0.1.0

IO layer for the transforms crate with ROS2 and Minot.
Documentation 
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

//! Conversion utilities for ROS messages.

use nalgebra::geometry::{Isometry3, Translation3, UnitQuaternion};
use transforms::geometry::{Quaternion, Transform, Vector3};
use transforms::time::Timestamp;

/// Convert nalgebra Isometry3 to Transform.
pub fn isometry_to_transform(
    iso: &Isometry3<f64>,
    parent: &str,
    child: &str,
    timestamp: Timestamp,
) -> Transform {
    Transform {
        translation: Vector3::new(iso.translation.x, iso.translation.y, iso.translation.z),
        rotation: Quaternion {
            w: iso.rotation.quaternion().w,
            x: iso.rotation.quaternion().i,
            y: iso.rotation.quaternion().j,
            z: iso.rotation.quaternion().k,
        },
        timestamp,
        parent: parent.to_string(),
        child: child.to_string(),
    }
}

/// Convert Transform to nalgebra Isometry3.
pub fn transform_to_isometry(tf: &Transform) -> Isometry3<f64> {
    Isometry3::from_parts(
        Translation3::new(tf.translation.x, tf.translation.y, tf.translation.z),
        UnitQuaternion::new_normalize(nalgebra::geometry::Quaternion::new(
            tf.rotation.w,
            tf.rotation.x,
            tf.rotation.y,
            tf.rotation.z,
        )),
    )
}