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/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)]