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

cu_v4l/
lib.rs
