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

cu_v4l/
lib.rs
