Add simple camera-capture with nokhwa.

There might be better camera capture crates, this
is just the first pick for now.

Signed-off-by: Henner Zeller <h.zeller@acm.org>

hostcontrol/src/camera.rs

@@ -0,0 +1,32 @@
+use anyhow::{Context, Result};
+use image::RgbImage;
+use nokhwa::Camera as NokhwaCamera;
+use nokhwa::pixel_format::RgbFormat;
+use nokhwa::utils::{CameraIndex, RequestedFormat, RequestedFormatType};
+
+pub struct Camera {
+    camera: NokhwaCamera,
+}
+impl Camera  {
+    pub fn new(camera_index: u32) -> Result<Self> {
+	let index = CameraIndex::Index(camera_index);
+        let format =
+            RequestedFormat::new::<RgbFormat>(RequestedFormatType::AbsoluteHighestResolution);
+        let mut camera = NokhwaCamera::new(index, format).context("could not find/access webcam")?;
+        camera.open_stream().context("failed to open stream")?;
+
+	Ok(Self {
+	    camera,
+	})
+    }
+
+    pub fn capture(&mut self) -> Result<RgbImage> {
+	for _ in 0..5 {
+	    // Hack to have camera flush frames in buffer.
+	    let _ = self.camera.frame();
+        }
+        let frame = self.camera.frame().context("Could not capture image")?;
+	println!("snap!");
+        Ok(frame.decode_image::<RgbFormat>()?)
+    }
+}

hostcontrol/src/gcode_stage.rs

@@ -28,6 +28,7 @@ impl XYStage for GCodeStage {
             a[1] / 1000.0,
             a[2] / 1000.0
         ))?;
+        self.stage_io.send_request("G4 P1\n")?; // make sure move finished.
         Ok(())
     }
 

hostcontrol/src/main.rs

@@ -1,12 +1,15 @@
 use clap::{Parser, ValueEnum};
-use std::{thread::sleep, time::Duration};
+use std::path::PathBuf;
+use image::RgbImage;
 
 mod gcode_stage;
 mod openflexure_stage;
+mod camera;
 
 mod stage_io;
 use crate::stage_io::StageIO;
 use crate::xy_stage::XYStage;
+use crate::camera::Camera;
 
 mod xy_stage;
 
@@ -21,8 +24,17 @@ struct CliArgs {
     #[arg(long, default_value = "115200")]
     tty_speed: u32,
 
+    /// Stage movement backend
     #[arg(long, value_enum)]
     backend: Backend,
+
+    /// Id of camera to fetch images from.
+    #[arg(long, default_value = "0")]
+    camera_index: u32,
+
+    /// Directory all captured images are stored.
+    #[arg(long, value_name="out-dir")]
+    output_directory: PathBuf,
 }
 
 // [derive(DebugCopy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)]
@@ -35,9 +47,29 @@ enum Backend {
     GCode,
 }
 
+fn store_image(number: u32, pos: nalgebra::Vector3<f64>, dir: &PathBuf,
+	       img: &RgbImage) {
+    let img_file =
+        &dir.join(format!("img-{:05}-{:0},{:0}.png", number, pos[0], pos[1]));
+
+    img.save(img_file).unwrap();
+}
+
 fn main() {
-    let args = CliArgs::parse();
     env_logger::init();
+    let args = CliArgs::parse();
+
+    if !args.output_directory.is_dir() {
+	panic!("--out-dir needs to be an existing directory");
+    }
+
+    let origin = nalgebra::vector![0.0, 0.0, 0.0];
+
+    // Picture source
+    let mut cam = Camera::new(args.camera_index).unwrap();
+    cam.capture().unwrap();  // Warm up camera.
+
+    // Movement
     let stage_io = StageIO::new(&args.stage_device, args.tty_speed).unwrap();
     let mut stage: Box<dyn XYStage> = match args.backend {
         Backend::GCode => Box::new(gcode_stage::GCodeStage::new(stage_io).unwrap()),
@@ -45,13 +77,17 @@ fn main() {
             Box::new(openflexure_stage::OpenFlexureStage::new(stage_io, 1.12).unwrap())
         }
     };
-    let origin = nalgebra::vector![0.0, 0.0, 0.0];
-    stage.move_absolute_cartesian(origin).unwrap();
-    sleep(Duration::from_secs(1));
+
+    // Local testing. Just take a line of pictures.
     let mut max_xy = stage.get_range();
+    // Only interested in lerp-ing X for now.
+    max_xy[1] = 0.0;
     max_xy[2] = 0.0;
-    stage.move_absolute_cartesian(max_xy).unwrap();
-    sleep(Duration::from_secs(5));
+
+    for n in 0..10 {
+	stage.move_absolute_cartesian(max_xy * n as f64 / 10.0).unwrap();
+	store_image(n, origin, &args.output_directory, &cam.capture().unwrap());
+    }
 }
 
 #[derive(Default)]