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

cu_v4l/
lib.rs
