uvt 0.1.1

Utilities for interacting with Uncrewed Vehicle Trajectory (UVT) files.
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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

//! This module provides functionality to parse trajectory data from ROS messages.
use crate::deserialization::BufferReader;
use crate::pose;
use std::io;

/// Trait for deserializing Trajectory messages.
///# Methods
///* `read_position` - Reads a Point representing position from the buffer.
///* `read_orientation` - Reads a Quaternion representing orientation from the buffer.
///* `read_covariance` - Reads a vector of f64 representing covariance from the buffer.
///* `read_vector` - Reads a Vector3 from the buffer.
pub trait TrajectoryDeserializer: BufferReader {
    fn read_position(&mut self) -> Result<pose::Point, io::Error>;
    fn read_orientation(&mut self) -> Result<pose::Quaternion, io::Error>;
    fn read_covariance(&mut self) -> Result<Vec<f64>, io::Error>;
    fn read_vector(&mut self) -> Result<pose::Vector3, io::Error>;
}

/// Parses a trajectory message from raw data into a sequence of PoseStamped instances.
///
/// # Arguments
///
/// * `deserializer` - An instance that converts the raw message into structured data.
///
/// # Returns
///
/// A vector of PoseStamped elements representing the trajectory.
pub fn parse_trajectory<D: TrajectoryDeserializer>(
    mut d: D,
) -> Result<pose::PoseStamped, std::io::Error> {
    // Message header
    let header = d.read_header()?;

    let _child_frame = d.read_lp_string_aligned(8)?;

    // Message pose
    let position = d.read_position()?;
    let orientation = d.read_orientation()?;

    // TODO: Implement PoseWithCovarianceStamped
    // https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html

    // Pose covariance
    // 6 x 6 covariance matrix = 36 covariance values
    let _pose_covariance = d.read_covariance()?;

    // Twist values
    let _twist_linear = d.read_vector()?;
    let _twist_angular = d.read_vector()?;

    // Twist covariance
    // 6 x 6 covariance matrix = 36 covariance values
    let _twist_covariance = d.read_covariance()?;

    Ok(pose::PoseStamped {
        header: header,
        pose: pose::Pose {
            position: position,
            orientation: orientation,
        },
    })
}