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

cu_v4l/
lib.rs
