Skip to main content

cu_vitfly/
task.rs

1use crate::{DEPTH_SHAPE, INPUT_HEIGHT, INPUT_WIDTH, VitFly, VitFlyState};
2use candle_core::{Device, Tensor};
3use cu_ahrs::AhrsPose;
4use cu_zed::ZedDepthMap;
5use cu29::bincode::{Decode, Encode};
6use cu29::prelude::*;
7use cu29::units::si::angle::radian;
8use cu29::units::si::f32::Velocity;
9use cu29::units::si::length::meter;
10use cu29::units::si::velocity::meter_per_second;
11
12/// Open-loop world-frame velocity predicted by ViTFly, in `[forward, left, up]` order.
13pub type VitFlyVelocity = [Velocity; 3];
14
15const DEFAULT_MAX_DEPTH_M: f32 = 12.5;
16const DEFAULT_INVALID_DEPTH: f32 = 0.8;
17const RECURRENT_VALUES: usize = 3 * 128;
18
19/// Copper task for the pretrained ViTFly depth policy.
20///
21/// The task consumes the standard ZED depth raster, Copper's AHRS pose, and a
22/// unit-safe desired speed. It resizes and normalizes the depth map, maintains
23/// the recurrent state, and emits a unit-safe XYZ velocity vector.
24#[derive(Reflect)]
25#[reflect(from_reflect = false)]
26pub struct VitFlyTask {
27    #[reflect(ignore)]
28    model: VitFly,
29    #[reflect(ignore)]
30    device: Device,
31    #[reflect(ignore)]
32    recurrent: Option<VitFlyState>,
33    #[reflect(ignore)]
34    resized_depth: Vec<f32>,
35    max_depth_m: f32,
36    invalid_depth: f32,
37}
38
39impl VitFlyTask {
40    fn from_config(config: Option<&ComponentConfig>) -> CuResult<Self> {
41        let max_depth_m = config_f32(config, "max_depth_m", DEFAULT_MAX_DEPTH_M)?;
42        if !max_depth_m.is_finite() || max_depth_m <= 0.0 {
43            return Err(CuError::from(
44                "vitfly max_depth_m must be finite and positive",
45            ));
46        }
47        let invalid_depth = config_f32(config, "invalid_depth", DEFAULT_INVALID_DEPTH)?;
48        if !invalid_depth.is_finite() {
49            return Err(CuError::from("vitfly invalid_depth must be finite"));
50        }
51
52        let requested_device = config
53            .map(|cfg| cfg.get::<String>("device"))
54            .transpose()?
55            .flatten();
56        let cuda_ordinal = config
57            .map(|cfg| cfg.get::<u32>("cuda_ordinal"))
58            .transpose()?
59            .flatten()
60            .unwrap_or(0) as usize;
61        let device = select_device(requested_device.as_deref(), cuda_ordinal)?;
62        let model = VitFly::load(&device)
63            .map_err(|err| CuError::new_with_cause("failed to load ViTFly model", err))?;
64
65        Ok(Self {
66            model,
67            device,
68            recurrent: None,
69            resized_depth: vec![0.0; INPUT_HEIGHT * INPUT_WIDTH],
70            max_depth_m,
71            invalid_depth,
72        })
73    }
74
75    fn run_model(
76        &mut self,
77        depth: &ZedDepthMap,
78        pose: &AhrsPose,
79        desired_speed: Velocity,
80    ) -> CuResult<VitFlyVelocity> {
81        let format = depth.format;
82        if format.width == 0
83            || format.height == 0
84            || !format.is_valid()
85            || format.required_elements() == 0
86        {
87            return Err(CuError::from(
88                "vitfly received an invalid ZED raster format",
89            ));
90        }
91
92        depth.with_samples(|samples, format| {
93            resize_and_normalize_depth(
94                samples,
95                format.width as usize,
96                format.height as usize,
97                format.stride as usize,
98                self.max_depth_m,
99                self.invalid_depth,
100                &mut self.resized_depth,
101            );
102        });
103
104        let speed_mps = desired_speed.get::<meter_per_second>();
105        if !speed_mps.is_finite() || speed_mps < 0.0 {
106            return Err(CuError::from(
107                "vitfly desired velocity must be finite and non-negative",
108            ));
109        }
110
111        let quaternion = euler_to_scalar_first_quaternion(
112            pose.roll.get::<radian>(),
113            pose.pitch.get::<radian>(),
114            pose.yaw.get::<radian>(),
115        );
116        let depth = Tensor::from_slice(&self.resized_depth, &DEPTH_SHAPE, &self.device)
117            .map_err(|err| CuError::new_with_cause("failed to upload ViTFly depth", err))?;
118        let desired_velocity = Tensor::new(&[[speed_mps]], &self.device)
119            .map_err(|err| CuError::new_with_cause("failed to upload ViTFly speed", err))?;
120        let attitude = Tensor::new(&[quaternion], &self.device)
121            .map_err(|err| CuError::new_with_cause("failed to upload ViTFly attitude", err))?;
122        let (prediction, recurrent) = self
123            .model
124            .forward(
125                &depth,
126                &desired_velocity,
127                &attitude,
128                self.recurrent.as_ref(),
129            )
130            .map_err(|err| CuError::new_with_cause("ViTFly inference failed", err))?;
131        let prediction = prediction
132            .to_vec2::<f32>()
133            .map_err(|err| CuError::new_with_cause("failed to download ViTFly output", err))?;
134        self.recurrent = Some(recurrent);
135
136        let [forward, left, up]: [f32; 3] = prediction
137            .first()
138            .and_then(|row| row.as_slice().try_into().ok())
139            .ok_or_else(|| CuError::from("ViTFly returned an invalid output shape"))?;
140        Ok([forward, left, up]
141            .map(|component| Velocity::new::<meter_per_second>(component * speed_mps)))
142    }
143}
144
145impl Freezable for VitFlyTask {
146    fn freeze<E: cu29::bincode::enc::Encoder>(
147        &self,
148        encoder: &mut E,
149    ) -> Result<(), cu29::bincode::error::EncodeError> {
150        let snapshot = self
151            .recurrent
152            .as_ref()
153            .map(|state| {
154                let hidden = state.hidden_tensor()?.flatten_all()?.to_vec1::<f32>()?;
155                let cell = state.cell_tensor()?.flatten_all()?.to_vec1::<f32>()?;
156                candle_core::Result::Ok((hidden, cell))
157            })
158            .transpose()
159            .map_err(|err| cu29::bincode::error::EncodeError::OtherString(err.to_string()))?;
160        Encode::encode(&snapshot, encoder)
161    }
162
163    fn thaw<D: cu29::bincode::de::Decoder>(
164        &mut self,
165        decoder: &mut D,
166    ) -> Result<(), cu29::bincode::error::DecodeError> {
167        let snapshot: Option<(Vec<f32>, Vec<f32>)> = Decode::decode(decoder)?;
168        self.recurrent = snapshot
169            .map(|(hidden, cell)| {
170                if hidden.len() != RECURRENT_VALUES || cell.len() != RECURRENT_VALUES {
171                    return Err(cu29::bincode::error::DecodeError::Other(
172                        "invalid ViTFly recurrent state length",
173                    ));
174                }
175                let hidden = Tensor::from_vec(hidden, (3, 128), &self.device)
176                    .map_err(candle_decode_error)?;
177                let cell =
178                    Tensor::from_vec(cell, (3, 128), &self.device).map_err(candle_decode_error)?;
179                VitFlyState::from_tensors(&hidden, &cell).map_err(candle_decode_error)
180            })
181            .transpose()?;
182        Ok(())
183    }
184}
185
186impl CuTask for VitFlyTask {
187    type Resources<'r> = ();
188    type Input<'m> = input_msg!('m, ZedDepthMap, AhrsPose, Velocity);
189    type Output<'m> = output_msg!(VitFlyVelocity);
190
191    fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self> {
192        Self::from_config(config)
193    }
194
195    fn process(
196        &mut self,
197        _ctx: &CuContext,
198        input: &Self::Input<'_>,
199        output: &mut Self::Output<'_>,
200    ) -> CuResult<()> {
201        let (depth_msg, pose_msg, speed_msg) = *input;
202        output.tov = depth_msg.tov;
203        let (Some(depth), Some(pose), Some(speed)) = (
204            depth_msg.payload(),
205            pose_msg.payload(),
206            speed_msg.payload().copied(),
207        ) else {
208            output.clear_payload();
209            output.metadata.set_status("missing input");
210            return Ok(());
211        };
212
213        output.set_payload(self.run_model(depth, pose, speed)?);
214        output.metadata.set_status("ok");
215        Ok(())
216    }
217}
218
219fn select_device(requested: Option<&str>, cuda_ordinal: usize) -> CuResult<Device> {
220    let requested = requested.unwrap_or(if cfg!(feature = "cuda") {
221        "cuda"
222    } else {
223        "cpu"
224    });
225    match requested {
226        "cpu" => Ok(Device::Cpu),
227        "cuda" => {
228            #[cfg(feature = "cuda")]
229            {
230                Device::new_cuda(cuda_ordinal)
231                    .map_err(|err| CuError::new_with_cause("failed to initialize CUDA", err))
232            }
233            #[cfg(not(feature = "cuda"))]
234            {
235                let _ = cuda_ordinal;
236                Err(CuError::from(
237                    "vitfly device=cuda requires the cu-vitfly cuda feature",
238                ))
239            }
240        }
241        _ => Err(CuError::from("vitfly device must be either cpu or cuda")),
242    }
243}
244
245fn config_f32(config: Option<&ComponentConfig>, key: &str, default: f32) -> CuResult<f32> {
246    Ok(config
247        .map(|cfg| cfg.get::<f32>(key))
248        .transpose()?
249        .flatten()
250        .unwrap_or(default))
251}
252
253fn candle_decode_error(err: candle_core::Error) -> cu29::bincode::error::DecodeError {
254    cu29::bincode::error::DecodeError::OtherString(err.to_string())
255}
256
257fn normalize_depth(sample: u16, max_depth_m: f32, invalid_depth: f32) -> f32 {
258    ZedDepthMap::decode_sample(sample)
259        .map(|depth| (depth.get::<meter>() / max_depth_m).clamp(0.0, 1.0))
260        .unwrap_or(invalid_depth)
261}
262
263#[allow(clippy::too_many_arguments)]
264fn resize_and_normalize_depth(
265    source: &[u16],
266    source_width: usize,
267    source_height: usize,
268    source_stride: usize,
269    max_depth_m: f32,
270    invalid_depth: f32,
271    destination: &mut [f32],
272) {
273    debug_assert_eq!(destination.len(), INPUT_HEIGHT * INPUT_WIDTH);
274    let scale_x = source_width as f32 / INPUT_WIDTH as f32;
275    let scale_y = source_height as f32 / INPUT_HEIGHT as f32;
276
277    for output_y in 0..INPUT_HEIGHT {
278        let source_y =
279            ((output_y as f32 + 0.5) * scale_y - 0.5).clamp(0.0, (source_height - 1) as f32);
280        let y0 = source_y.floor() as usize;
281        let y1 = (y0 + 1).min(source_height - 1);
282        let wy = source_y - y0 as f32;
283        for output_x in 0..INPUT_WIDTH {
284            let source_x =
285                ((output_x as f32 + 0.5) * scale_x - 0.5).clamp(0.0, (source_width - 1) as f32);
286            let x0 = source_x.floor() as usize;
287            let x1 = (x0 + 1).min(source_width - 1);
288            let wx = source_x - x0 as f32;
289            let top_left =
290                normalize_depth(source[y0 * source_stride + x0], max_depth_m, invalid_depth);
291            let top_right =
292                normalize_depth(source[y0 * source_stride + x1], max_depth_m, invalid_depth);
293            let bottom_left =
294                normalize_depth(source[y1 * source_stride + x0], max_depth_m, invalid_depth);
295            let bottom_right =
296                normalize_depth(source[y1 * source_stride + x1], max_depth_m, invalid_depth);
297            let top = top_left * (1.0 - wx) + top_right * wx;
298            let bottom = bottom_left * (1.0 - wx) + bottom_right * wx;
299            destination[output_y * INPUT_WIDTH + output_x] = top * (1.0 - wy) + bottom * wy;
300        }
301    }
302}
303
304fn euler_to_scalar_first_quaternion(roll: f32, pitch: f32, yaw: f32) -> [f32; 4] {
305    let (sr, cr) = (0.5 * roll).sin_cos();
306    let (sp, cp) = (0.5 * pitch).sin_cos();
307    let (sy, cy) = (0.5 * yaw).sin_cos();
308    [
309        cr * cp * cy + sr * sp * sy,
310        sr * cp * cy - cr * sp * sy,
311        cr * sp * cy + sr * cp * sy,
312        cr * cp * sy - sr * sp * cy,
313    ]
314}
315
316#[cfg(test)]
317mod tests {
318    use super::*;
319
320    #[test]
321    fn identity_pose_maps_to_scalar_first_identity() {
322        assert_eq!(
323            euler_to_scalar_first_quaternion(0.0, 0.0, 0.0),
324            [1.0, 0.0, 0.0, 0.0]
325        );
326    }
327
328    #[test]
329    fn resize_respects_stride_and_normalizes_meters() {
330        let mut source = vec![u16::MAX; 6 * 2];
331        source[..4].copy_from_slice(&[0, 6_250, 12_500, 25_000]);
332        source[6..10].copy_from_slice(&[0, 6_250, 12_500, 25_000]);
333        let mut destination = vec![0.0; INPUT_HEIGHT * INPUT_WIDTH];
334        resize_and_normalize_depth(&source, 4, 2, 6, 12.5, 0.8, &mut destination);
335        assert_eq!(destination[0], 0.8);
336        assert!((destination[INPUT_WIDTH - 1] - 1.0).abs() < 1.0e-6);
337    }
338}