cu_v4l/
lib.rs

1#[cfg(target_os = "linux")]
2mod v4lstream;
3
4// This allows this module to be used on simulation on Windows and MacOS
5#[cfg(not(target_os = "linux"))]
6mod empty_impl {
7    use cu_sensor_payloads::CuImage;
8    use cu29::prelude::*;
9
10    #[derive(Reflect)]
11    pub struct V4l {}
12
13    impl Freezable for V4l {}
14
15    impl CuSrcTask for V4l {
16        type Resources<'r> = ();
17        type Output<'m> = output_msg!(CuImage<Vec<u8>>);
18
19        fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
20        where
21            Self: Sized,
22        {
23            Ok(Self {})
24        }
25
26        fn process(&mut self, _ctx: &CuContext, _new_msg: &mut Self::Output<'_>) -> CuResult<()> {
27            Ok(())
28        }
29    }
30}
31
32#[cfg(not(target_os = "linux"))]
33pub use empty_impl::V4l;
34
35#[cfg(target_os = "linux")]
36pub use linux_impl::V4l;
37
38#[cfg(target_os = "linux")]
39mod linux_impl {
40    use std::time::Duration;
41    use v4l::video::Capture;
42
43    use crate::v4lstream::CuV4LStream;
44    use cu_sensor_payloads::{CuImage, CuImageBufferFormat};
45    use cu29::prelude::*;
46
47    use nix::time::{ClockId, clock_gettime};
48
49    pub use v4l::buffer::Type;
50    pub use v4l::framesize::FrameSizeEnum;
51    pub use v4l::io::traits::{CaptureStream, Stream};
52    pub use v4l::prelude::*;
53    pub use v4l::video::capture::Parameters;
54    pub use v4l::{Format, FourCC, Timestamp};
55
56    // A Copper source task that reads frames from a V4L device.
57    #[derive(Reflect)]
58    #[reflect(from_reflect = false)]
59    pub struct V4l {
60        #[reflect(ignore)]
61        stream: CuV4LStream,
62        #[reflect(ignore)]
63        settled_format: CuImageBufferFormat,
64        v4l_clock_time_offset_ns: i64,
65    }
66
67    impl Freezable for V4l {}
68
69    fn cutime_from_v4ltime(offset_ns: i64, v4l_time: Timestamp) -> CuTime {
70        let duration: Duration = v4l_time.into();
71        ((duration.as_nanos() as i64 + offset_ns) as u64).into()
72    }
73
74    impl CuSrcTask for V4l {
75        type Resources<'r> = ();
76        type Output<'m> = output_msg!(CuImage<Vec<u8>>);
77
78        fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
79        where
80            Self: Sized,
81        {
82            // reasonable defaults
83            let mut v4l_device = 0usize;
84            let mut req_width: Option<u32> = None;
85            let mut req_height: Option<u32> = None;
86            let mut req_fps: Option<u32> = None;
87            let mut req_fourcc: Option<String> = None;
88            let mut req_buffers: u32 = 4;
89            let mut req_timeout: Duration = Duration::from_millis(500); // 500ms tolerance to get a frame
90
91            if let Some(config) = _config {
92                if let Some(device) = config.get::<u32>("device")? {
93                    v4l_device = device as usize;
94                }
95                if let Some(width) = config.get::<u32>("width")? {
96                    req_width = Some(width);
97                }
98                if let Some(height) = config.get::<u32>("height")? {
99                    req_height = Some(height);
100                }
101                if let Some(fps) = config.get::<u32>("fps")? {
102                    req_fps = Some(fps);
103                }
104                if let Some(fourcc) = config.get::<String>("fourcc")? {
105                    req_fourcc = Some(fourcc);
106                }
107                if let Some(buffers) = config.get::<u32>("buffers")? {
108                    req_buffers = buffers;
109                }
110                if let Some(timeout) = config.get::<u32>("timeout_ms")? {
111                    req_timeout = Duration::from_millis(timeout as u64);
112                }
113            }
114            let dev = Device::new(v4l_device)
115                .map_err(|e| CuError::new_with_cause("Failed to open camera", e))?;
116
117            // List all formats supported by the device
118            let formats = dev
119                .enum_formats()
120                .map_err(|e| CuError::new_with_cause("Failed to enum formats", e))?;
121
122            if formats.is_empty() {
123                return Err("The V4l device did not provide any video format.".into());
124            }
125
126            // Either use the 4CC or just pick one for the user
127            let fourcc: FourCC = if let Some(fourcc) = req_fourcc {
128                if fourcc.len() != 4 {
129                    return Err("Invalid fourcc provided".into());
130                }
131                FourCC::new(fourcc.as_bytes()[0..4].try_into().unwrap())
132            } else {
133                debug!("No fourcc provided, just use the first one we can find.");
134                formats.first().unwrap().fourcc
135            };
136            debug!("V4L: Using fourcc: {}", fourcc.to_string());
137            let actual_fmt = if let Some(format) = formats.iter().find(|f| f.fourcc == fourcc) {
138                // Enumerate resolutions for the BGR3 format
139                let resolutions = dev
140                    .enum_framesizes(format.fourcc)
141                    .map_err(|e| CuError::new_with_cause("Failed to enum frame sizes", e))?;
142                let (width, height) =
143                    if let (Some(req_width), Some(req_height)) = (req_width, req_height) {
144                        let mut frame_size: (u32, u32) = (0, 0);
145                        for frame in resolutions.iter() {
146                            let FrameSizeEnum::Discrete(size) = &frame.size else {
147                                todo!()
148                            };
149                            if size.width == req_width && size.height == req_height {
150                                frame_size = (size.width, size.height);
151                                break;
152                            }
153                        }
154                        frame_size
155                    } else {
156                        // just pick the first available
157                        let fs = resolutions.first().unwrap();
158                        let FrameSizeEnum::Discrete(size) = &fs.size else {
159                            todo!()
160                        };
161                        (size.width, size.height)
162                    };
163
164                // Set the format with the chosen resolution
165                let req_fmt = Format::new(width, height, fourcc);
166                let actual_fmt = dev
167                    .set_format(&req_fmt)
168                    .map_err(|e| CuError::new_with_cause("Failed to set format", e))?;
169
170                if let Some(fps) = req_fps {
171                    debug!("V4L: Set fps to {}", fps);
172                    let new_params = Parameters::with_fps(fps);
173                    dev.set_params(&new_params)
174                        .map_err(|e| CuError::new_with_cause("Failed to set params", e))?;
175                }
176                debug!(
177                    "V4L: Negotiated resolution: {}x{}",
178                    actual_fmt.width, actual_fmt.height
179                );
180                actual_fmt
181            } else {
182                return Err(format!(
183                    "The V4l device {v4l_device} does not provide a format with the FourCC {fourcc}."
184                )
185                .into());
186            };
187            debug!(
188                "V4L: Init stream: device {} with {} buffers of size {} bytes",
189                v4l_device, req_buffers, actual_fmt.size
190            );
191
192            let mut stream = CuV4LStream::with_buffers(
193                &dev,
194                Type::VideoCapture,
195                req_buffers,
196                CuHostMemoryPool::new(
197                    format!("V4L Host Pool {v4l_device}").as_str(),
198                    req_buffers as usize + 1,
199                    || vec![0; actual_fmt.size as usize],
200                )
201                .map_err(|e| {
202                    CuError::new_with_cause(
203                        "Could not create host memory pool backing the V4lStream",
204                        e,
205                    )
206                })?,
207            )
208            .map_err(|e| CuError::new_with_cause("Could not create the V4lStream", e))?;
209            let req_timeout_ms = req_timeout.as_millis() as u64;
210            debug!("V4L: Set timeout to {} ms", req_timeout_ms);
211            stream.set_timeout(req_timeout);
212
213            let cuformat = CuImageBufferFormat {
214                width: actual_fmt.width,
215                height: actual_fmt.height,
216                stride: actual_fmt.stride,
217                pixel_format: actual_fmt.fourcc.repr,
218            };
219
220            Ok(Self {
221                stream,
222                settled_format: cuformat,
223                v4l_clock_time_offset_ns: 0, // will be set at start
224            })
225        }
226
227        fn start(&mut self, ctx: &CuContext) -> CuResult<()> {
228            let rb_ns = ctx.now().as_nanos();
229            clock_gettime(ClockId::CLOCK_MONOTONIC)
230                .map(|ts| {
231                    self.v4l_clock_time_offset_ns =
232                        ts.tv_sec() * 1_000_000_000 + ts.tv_nsec() - rb_ns as i64
233                })
234                .map_err(|e| CuError::new_with_cause("Failed to get the current time", e))?;
235
236            self.stream
237                .start()
238                .map_err(|e| CuError::new_with_cause("could not start stream", e))
239        }
240
241        fn process(&mut self, _ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
242            let (handle, meta) = self
243                .stream
244                .next()
245                .map_err(|e| CuError::new_with_cause("could not get next frame from stream", e))?;
246            if meta.bytesused != 0 {
247                let cutime = cutime_from_v4ltime(self.v4l_clock_time_offset_ns, meta.timestamp);
248                let image = CuImage::new(self.settled_format, handle.clone());
249                new_msg.set_payload(image);
250                new_msg.tov = Tov::Time(cutime);
251            } else {
252                debug!("Empty frame received");
253            }
254            Ok(())
255        }
256
257        fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
258            self.stream
259                .stop()
260                .map_err(|e| CuError::new_with_cause("could not stop stream", e))
261        }
262    }
263
264    #[cfg(test)]
265    mod tests {
266        use super::*;
267        use rerun::RecordingStreamBuilder;
268        use rerun::components::ImageBuffer;
269        use rerun::datatypes::{Blob, ImageFormat};
270        use rerun::{Image, PixelFormat};
271        use std::thread;
272
273        use simplelog::{ColorChoice, Config, LevelFilter, TermLogger, TerminalMode};
274
275        const IMG_WIDTH: usize = 3840;
276        const IMG_HEIGHT: usize = 2160;
277
278        #[derive(Debug)]
279        struct NullLog {}
280        impl WriteStream<CuLogEntry> for NullLog {
281            fn log(&mut self, _obj: &CuLogEntry) -> CuResult<()> {
282                Ok(())
283            }
284            fn flush(&mut self) -> CuResult<()> {
285                Ok(())
286            }
287        }
288
289        #[test]
290        #[ignore]
291        fn emulate_copper_backend() {
292            let ctx = CuContext::new_with_clock();
293
294            let term_logger = TermLogger::new(
295                LevelFilter::Debug,
296                Config::default(),
297                TerminalMode::Mixed,
298                ColorChoice::Auto,
299            );
300            let _logger = LoggerRuntime::init(ctx.clock.clone(), NullLog {}, Some(*term_logger));
301
302            let rec = RecordingStreamBuilder::new("Camera Viz")
303                .spawn()
304                .map_err(|e| CuError::new_with_cause("Failed to spawn rerun stream", e))
305                .unwrap();
306
307            let mut config = ComponentConfig::new();
308            config.set("device", 0);
309            config.set("width", IMG_WIDTH as u32);
310            config.set("height", IMG_HEIGHT as u32);
311            config.set("fps", 30);
312            config.set("fourcc", "NV12".to_string());
313            config.set("buffers", 4);
314            config.set("timeout_ms", 500);
315
316            let mut v4l = V4l::new(Some(&config), ()).unwrap();
317            v4l.start(&ctx).unwrap();
318
319            let mut msg = CuMsg::new(None);
320            // Define the image format
321            let format = rerun::components::ImageFormat(ImageFormat {
322                width: IMG_WIDTH as u32,
323                height: IMG_HEIGHT as u32,
324                pixel_format: Some(PixelFormat::NV12),
325                color_model: None,      // Some(ColorModel::BGR),
326                channel_datatype: None, // Some(ChannelDatatype::U8),
327            });
328            for _ in 0..1000 {
329                let _output = v4l.process(&ctx, &mut msg);
330                if let Some(frame) = msg.payload() {
331                    let slice: &[u8] = &frame.buffer_handle.lock().unwrap();
332                    let blob = Blob::from(slice);
333                    let rerun_img = ImageBuffer::from(blob);
334                    let image = Image::new(rerun_img, format);
335
336                    rec.log("images", &image).unwrap();
337                } else {
338                    debug!("----> No frame");
339                    thread::sleep(Duration::from_millis(300)); // don't burn through empty buffers at the beginning, what for the device to actually start
340                }
341            }
342
343            v4l.stop(&ctx).unwrap();
344        }
345    }
346}
cu_v4l/lib.rs

cu_v4l/
lib.rs
