mecha10-core 0.8.2

Mecha10 core runtime and foundational types
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
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
140
141
142
143
144
145
146
147

use super::perception::Pose;
use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavigationGoal {
    pub timestamp: u64,

    pub goal_id: String,

    pub target: Pose,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_linear_velocity: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_angular_velocity: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub position_tolerance: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation_tolerance: Option<f32>,
}

impl NavigationGoal {
    /// Create a simple 2D navigation goal
    pub fn new_2d(goal_id: String, x: f32, y: f32, yaw: f32) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            goal_id,
            target: Pose::new_2d(x, y, yaw),
            max_linear_velocity: None,
            max_angular_velocity: None,
            position_tolerance: None,
            orientation_tolerance: None,
        }
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Path {
    pub timestamp: u64,

    pub waypoints: Vec<Waypoint>,

    pub total_length: f32,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub estimated_time: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

impl Path {
    /// Create an empty path
    pub fn empty() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            waypoints: Vec::new(),
            total_length: 0.0,
            estimated_time: None,
            frame_id: None,
        }
    }

    /// Check if path is empty
    pub fn is_empty(&self) -> bool {
        self.waypoints.is_empty()
    }

    /// Get the number of waypoints
    pub fn len(&self) -> usize {
        self.waypoints.len()
    }

    /// Calculate total path length from waypoints
    pub fn calculate_length(&mut self) {
        let mut length = 0.0;

        for i in 1..self.waypoints.len() {
            let prev = &self.waypoints[i - 1];
            let curr = &self.waypoints[i];

            let dx = curr.pose.position[0] - prev.pose.position[0];
            let dy = curr.pose.position[1] - prev.pose.position[1];
            let dz = curr.pose.position[2] - prev.pose.position[2];

            length += (dx * dx + dy * dy + dz * dz).sqrt();
        }

        self.total_length = length;
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Waypoint {
    pub pose: Pose,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub velocity: Option<f32>,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub distance_along_path: Option<f32>,
}

impl Waypoint {
    /// Create a simple waypoint from 2D coordinates
    pub fn new_2d(x: f32, y: f32, yaw: f32) -> Self {
        Self {
            pose: Pose::new_2d(x, y, yaw),
            velocity: None,
            distance_along_path: None,
        }
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavigationStatus {
    pub timestamp: u64,

    pub goal_id: String,

    pub state: NavigationState,

    pub distance_remaining: f32,

    #[serde(skip_serializing_if = "Option::is_none")]
    pub time_remaining: Option<f32>,

    pub progress: f32,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum NavigationState {
    Planning,

    Moving,

    Succeeded,

    Failed,

    Waiting,

    Recovering,
}