diff --git a/playbooks/files/defaultConfig.yml b/playbooks/files/defaultConfig.yml index 2a66f36..1328950 100644 --- a/playbooks/files/defaultConfig.yml +++ b/playbooks/files/defaultConfig.yml @@ -4,6 +4,8 @@ launch: serialPort: /dev/ttyACM0 baudRate: 115200 heartbeatRate: 1 + fileReceiverPort: 12347 + fileReceiverSocketBacklog: 100 topsides: pilotPanelName: Pilot Panel pilotPanelPort: /dev/ttyACM0 @@ -92,8 +94,10 @@ cameraCalibration: cameraBImagesDirectory: /home/eedge/calibration-b/ cameraAValidImagesDirectory: /home/eedge/valid-calibration-a/ cameraBValidImagesDirectory: /home/eedge/valid-calibration-b/ - chessboardWidth: 12 - chessboardHeight: 13 + cameraAPreUndistortedDirectory: /home/eedge/pre-undistorted-a/ + cameraBPreUndistortedDirectory: /home/eedge/pre-undistorted-b/ + chessboardWidth: 6 + chessboardHeight: 9 distanceCalculator: imageDirectory: /home/eedge/undistorted-images/ dataDirectory: /home/eedge/undistorted-images-data/ diff --git a/playbooks/files/etc/systemd/system/eer-camera.service b/playbooks/files/etc/systemd/system/eer-camera.service index 6e279a7..fdcc0d0 100644 --- a/playbooks/files/etc/systemd/system/eer-camera.service +++ b/playbooks/files/etc/systemd/system/eer-camera.service @@ -3,4 +3,4 @@ Description=Eastern Edge Robotics Camera Feed Software [Service] EnvironmentFile=/run/eer/camera-environment -ExecStart=/usr/bin/eer-camera-feed $host $port +ExecStart=/usr/bin/eer-camera-feed $host $video_port $image_port diff --git a/playbooks/files/usr/bin/eer-camera-feed b/playbooks/files/usr/bin/eer-camera-feed index c741e38..7a1e695 100644 --- a/playbooks/files/usr/bin/eer-camera-feed +++ b/playbooks/files/usr/bin/eer-camera-feed @@ -3,27 +3,53 @@ import picamera import signal import socket import sys +import threading + +host = sys.argv[1] +video_port = int(sys.argv[2]) +image_port = int(sys.argv[3]) + +signal_lock = threading.Lock() with picamera.PiCamera() as camera: def flip_video(signum, stack): - camera.vflip = not camera.vflip - camera.hflip = not camera.hflip + with signal_lock: + camera.vflip = not camera.vflip + camera.hflip = not camera.hflip signal.signal(signal.SIGUSR1, flip_video) - camera.resolution = (1296, 720) + + def take_image(signum, stack): + with signal_lock: + with open('/run/eer/camera-image-path', 'r') as image_path_file: + image_path = image_path_file.readlines()[0] + image_socket = socket.socket() + image_socket.connect((host, image_port)) + image_socket.send(image_path + '\n') + image_file = image_socket.makefile('wb') + try: + camera.capture(image_file, 'png') + finally: + image_file.close() + image_socket.close() + + signal.signal(signal.SIGUSR2, take_image) + + camera.resolution = (1920, 1088) camera.framerate = 24 - nc = socket.socket() - nc.connect((sys.argv[1], int(sys.argv[2]))) - fd = nc.makefile('wb') + video_socket = socket.socket() + video_socket.connect((host, video_port)) + video_file = video_socket.makefile('wb', 1024) try: camera.start_recording( - fd, + video_file, format='h264', intra_period=0, quality=0, - bitrate=25000000) + bitrate=25000000, + resize=(1296, 720)) while True: signal.pause() finally: - fd.close() - nc.close() + video_file.close() + video_socket.close() diff --git a/src/main/java/com/easternedgerobotics/rov/PicameraA.java b/src/main/java/com/easternedgerobotics/rov/PicameraA.java index bc85e44..45e9c73 100644 --- a/src/main/java/com/easternedgerobotics/rov/PicameraA.java +++ b/src/main/java/com/easternedgerobotics/rov/PicameraA.java @@ -4,6 +4,7 @@ import com.easternedgerobotics.rov.config.LaunchConfig; import com.easternedgerobotics.rov.event.BroadcastEventPublisher; import com.easternedgerobotics.rov.event.EventPublisher; +import com.easternedgerobotics.rov.value.CameraCaptureValueA; import com.easternedgerobotics.rov.value.PicameraAHeartbeatValue; import com.easternedgerobotics.rov.value.VideoFlipValueA; import com.easternedgerobotics.rov.value.VideoValueA; @@ -33,14 +34,27 @@ private PicameraA() { } - private static void initCameraA(final EventPublisher eventPublisher, final int heartbeatRate) { + private static void initCameraA( + final EventPublisher eventPublisher, + final int heartbeatRate, + final int fileReceiverPort + ) { eventPublisher.valuesOfType(VideoFlipValueA.class) + .observeOn(Schedulers.io()) .subscribe(f -> PicameraVideo.flip()); + + eventPublisher.valuesOfType(CameraCaptureValueA.class) + .observeOn(Schedulers.io()) + .subscribe(capture -> PicameraVideo.takeImage(capture.getPath())); + eventPublisher.valuesOfType(VideoValueA.class) - .throttleLast(1, TimeUnit.SECONDS) - .subscribe(value -> PicameraVideo.start(value.getHost(), value.getPort())); + .throttleLast(1, TimeUnit.SECONDS, Schedulers.io()) + .observeOn(Schedulers.io()) + .subscribe(value -> PicameraVideo.start(value.getHost(), value.getPort(), fileReceiverPort)); + Observable.interval(heartbeatRate, TimeUnit.SECONDS, Schedulers.io()) .map(t -> new PicameraAHeartbeatValue(true)) + .observeOn(Schedulers.io()) .subscribe(eventPublisher::emit); } @@ -81,7 +95,7 @@ public static void main(final String[] args) throws InterruptedException, Socket final EventPublisher eventPublisher = new BroadcastEventPublisher(new UdpBroadcast<>( socket, broadcastAddress, broadcastPort, new BasicOrder<>())); PicameraVideo.addShutdownHook(); - initCameraA(eventPublisher, launchConfig.heartbeatRate()); + initCameraA(eventPublisher, launchConfig.heartbeatRate(), launchConfig.fileReceiverPort()); Logger.info("Started"); eventPublisher.await(); } catch (final ParseException e) { diff --git a/src/main/java/com/easternedgerobotics/rov/PicameraB.java b/src/main/java/com/easternedgerobotics/rov/PicameraB.java index 04a7eed..2956634 100644 --- a/src/main/java/com/easternedgerobotics/rov/PicameraB.java +++ b/src/main/java/com/easternedgerobotics/rov/PicameraB.java @@ -4,6 +4,7 @@ import com.easternedgerobotics.rov.config.LaunchConfig; import com.easternedgerobotics.rov.event.BroadcastEventPublisher; import com.easternedgerobotics.rov.event.EventPublisher; +import com.easternedgerobotics.rov.value.CameraCaptureValueB; import com.easternedgerobotics.rov.value.PicameraBHeartbeatValue; import com.easternedgerobotics.rov.value.VideoFlipValueB; import com.easternedgerobotics.rov.value.VideoValueB; @@ -33,14 +34,27 @@ private PicameraB() { } - private static void initCameraB(final EventPublisher eventPublisher, final int heartbeatRate) { + private static void initCameraB( + final EventPublisher eventPublisher, + final int heartbeatRate, + final int fileReceiverPort + ) { eventPublisher.valuesOfType(VideoFlipValueB.class) + .observeOn(Schedulers.io()) .subscribe(f -> PicameraVideo.flip()); + + eventPublisher.valuesOfType(CameraCaptureValueB.class) + .observeOn(Schedulers.io()) + .subscribe(capture -> PicameraVideo.takeImage(capture.getPath())); + eventPublisher.valuesOfType(VideoValueB.class) - .throttleLast(1, TimeUnit.SECONDS) - .subscribe(value -> PicameraVideo.start(value.getHost(), value.getPort())); + .throttleLast(1, TimeUnit.SECONDS, Schedulers.io()) + .observeOn(Schedulers.io()) + .subscribe(value -> PicameraVideo.start(value.getHost(), value.getPort(), fileReceiverPort)); + Observable.interval(heartbeatRate, TimeUnit.SECONDS, Schedulers.io()) .map(t -> new PicameraBHeartbeatValue(true)) + .observeOn(Schedulers.io()) .subscribe(eventPublisher::emit); } @@ -81,7 +95,7 @@ public static void main(final String[] args) throws InterruptedException, Socket final EventPublisher eventPublisher = new BroadcastEventPublisher(new UdpBroadcast<>( socket, broadcastAddress, broadcastPort, new BasicOrder<>())); PicameraVideo.addShutdownHook(); - initCameraB(eventPublisher, launchConfig.heartbeatRate()); + initCameraB(eventPublisher, launchConfig.heartbeatRate(), launchConfig.fileReceiverPort()); Logger.info("Started"); eventPublisher.await(); } catch (final ParseException e) { diff --git a/src/main/java/com/easternedgerobotics/rov/Topside.java b/src/main/java/com/easternedgerobotics/rov/Topside.java index 506e3fe..7ba4cf0 100644 --- a/src/main/java/com/easternedgerobotics/rov/Topside.java +++ b/src/main/java/com/easternedgerobotics/rov/Topside.java @@ -18,6 +18,7 @@ import com.easternedgerobotics.rov.io.MotionPowerProfile; import com.easternedgerobotics.rov.io.ProfileController; import com.easternedgerobotics.rov.io.SliderController; +import com.easternedgerobotics.rov.io.TcpFileReceiver; import com.easternedgerobotics.rov.io.arduino.Arduino; import com.easternedgerobotics.rov.io.arduino.ArduinoPort; import com.easternedgerobotics.rov.io.joystick.JoystickController; @@ -60,6 +61,8 @@ public final class Topside extends Application { private JoystickController joystickController; + private TcpFileReceiver fileReceiver; + @Override public void init() throws SocketException, UnknownHostException { final Config configSource = new Config( @@ -104,10 +107,12 @@ public void init() throws SocketException, UnknownHostException { eventPublisher, configSource.getConfig("videoDecoder", VideoDecoderConfig.class)); cameraCalibration = new CameraCalibration( - videoDecoder, + eventPublisher, configSource.getConfig("cameraCalibration", CameraCalibrationConfig.class), Schedulers.newThread()); + fileReceiver = new TcpFileReceiver(launchConfig.fileReceiverPort(), launchConfig.fileReceiverSocketBacklog()); + viewLoader = new ViewLoader(MainView.class, "Control Software", new HashMap, Object>() { { put(EventPublisher.class, eventPublisher); @@ -134,6 +139,7 @@ public void start(final Stage stage) { arduino.start(config.pilotPanelHeartbeatInterval(), config.pilotPanelHeartbeatTimeout(), TimeUnit.MILLISECONDS); joystickController.start(LogitechExtremeJoystickSource.create( config.joystickRecoveryInterval(), TimeUnit.MILLISECONDS, Schedulers.io())); + fileReceiver.start(); Logger.info("Started"); } @@ -146,6 +152,7 @@ public void stop() { profileController.stop(); videoDecoder.stop(); joystickController.stop(); + fileReceiver.stop(); Logger.info("Stopped"); } diff --git a/src/main/java/com/easternedgerobotics/rov/config/CameraCalibrationConfig.java b/src/main/java/com/easternedgerobotics/rov/config/CameraCalibrationConfig.java index 838127e..e0bc03c 100644 --- a/src/main/java/com/easternedgerobotics/rov/config/CameraCalibrationConfig.java +++ b/src/main/java/com/easternedgerobotics/rov/config/CameraCalibrationConfig.java @@ -9,6 +9,10 @@ public interface CameraCalibrationConfig { String cameraBValidImagesDirectory(); + String cameraAPreUndistortedDirectory(); + + String cameraBPreUndistortedDirectory(); + int chessboardWidth(); int chessboardHeight(); diff --git a/src/main/java/com/easternedgerobotics/rov/config/LaunchConfig.java b/src/main/java/com/easternedgerobotics/rov/config/LaunchConfig.java index 1e871a8..63d1b74 100644 --- a/src/main/java/com/easternedgerobotics/rov/config/LaunchConfig.java +++ b/src/main/java/com/easternedgerobotics/rov/config/LaunchConfig.java @@ -10,4 +10,8 @@ public interface LaunchConfig { int baudRate(); int heartbeatRate(); + + int fileReceiverPort(); + + int fileReceiverSocketBacklog(); } diff --git a/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java b/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java index b2a4fc9..6c5e348 100644 --- a/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java +++ b/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java @@ -61,7 +61,6 @@ private void updateDirectory(final ObservableValue value, fina subscriptions.clear(); subscriptions.add(DirectoryUtil.observe(Paths.get(curr)) .subscribeOn(Schedulers.newThread()) - .delay(1, TimeUnit.SECONDS) .subscribe(this::onImagePathChanged)); } @@ -84,6 +83,10 @@ private void onImagePathChanged(final Path path) { }); return; } + // in the event that the file was not completed. + if (file.length() == 0) { + Schedulers.io().createWorker().schedule(() -> onImagePathChanged(path), 1, TimeUnit.SECONDS); + } try { final GalleryImageView iv = new GalleryImageView(path, view.actions); Platform.runLater(() -> { diff --git a/src/main/java/com/easternedgerobotics/rov/fx/MainViewController.java b/src/main/java/com/easternedgerobotics/rov/fx/MainViewController.java index 3240272..639a917 100644 --- a/src/main/java/com/easternedgerobotics/rov/fx/MainViewController.java +++ b/src/main/java/com/easternedgerobotics/rov/fx/MainViewController.java @@ -1,21 +1,22 @@ package com.easternedgerobotics.rov.fx; import com.easternedgerobotics.rov.config.Configurable; -import com.easternedgerobotics.rov.control.SourceController; import com.easternedgerobotics.rov.event.EventPublisher; import com.easternedgerobotics.rov.io.EmergencyStopController; +import com.easternedgerobotics.rov.value.HeartbeatValue; import com.easternedgerobotics.rov.value.PicameraAHeartbeatValue; import com.easternedgerobotics.rov.value.PicameraBHeartbeatValue; import com.easternedgerobotics.rov.value.RasprimeHeartbeatValue; import com.easternedgerobotics.rov.value.TopsideHeartbeatValue; import com.easternedgerobotics.rov.video.VideoDecoder; +import javafx.scene.control.ToggleButton; import rx.Observable; +import rx.Subscription; import rx.observables.JavaFxObservable; import rx.schedulers.Schedulers; import rx.subscriptions.CompositeSubscription; -import java.util.Arrays; import java.util.concurrent.TimeUnit; import javax.inject.Inject; @@ -73,19 +74,17 @@ public final void onCreate() { final Observable heartbeatLostInterval = Observable.interval( maxHeartbeatGap, TimeUnit.SECONDS, JAVA_FX_SCHEDULER); - Observable.zip( - Observable.from(Arrays.asList( - RasprimeHeartbeatValue.class, - PicameraAHeartbeatValue.class, - PicameraBHeartbeatValue.class)), - Observable.from(Arrays.asList( - view.rasprimeIndicator, - view.picameraAIndicator, - view.picameraBIndicator)), - (clazz, indicator) -> SourceController.manageMultiViewModel( - eventPublisher.valuesOfType(clazz), h -> indicator.setBackground(MainView.FOUND_BG), JAVA_FX_SCHEDULER, - heartbeatLostInterval, t -> indicator.setBackground(MainView.LOST_BG), JAVA_FX_SCHEDULER) - ).subscribe(subscriptions::add); + final Observable rasprimeHeartbeats = eventPublisher + .valuesOfType(RasprimeHeartbeatValue.class).cast(HeartbeatValue.class); + final Observable picameraAHeartbeats = eventPublisher + .valuesOfType(PicameraAHeartbeatValue.class).cast(HeartbeatValue.class); + final Observable picameraBHeartbeats = eventPublisher + .valuesOfType(PicameraBHeartbeatValue.class).cast(HeartbeatValue.class); + + subscriptions.addAll( + setIndicator(rasprimeHeartbeats, view.rasprimeIndicator), + setIndicator(picameraAHeartbeats, view.picameraAIndicator), + setIndicator(picameraBHeartbeats, view.picameraBIndicator)); JavaFxObservable.valuesOf(view.thrusterButton.pressedProperty()).filter(x -> !x) .subscribe(v -> viewLoader.load(ThrusterPowerSlidersView.class, "Thruster Power")); @@ -107,6 +106,15 @@ public final void onDestroy() { eventPublisher.emit(new TopsideHeartbeatValue(false)); } + private Subscription setIndicator(final Observable heartbeats, final ToggleButton indicator) { + final Observable timeout = Observable.just(false) + .delay(maxHeartbeatGap, TimeUnit.SECONDS, JAVA_FX_SCHEDULER) + .concatWith(Observable.never()); + return new CompositeSubscription( + timeout.takeUntil(heartbeats).repeat().subscribe(h -> indicator.setBackground(MainView.LOST_BG)), + heartbeats.subscribe(h -> indicator.setBackground(MainView.FOUND_BG))); + } + private void onSelected(final boolean selected) { if (selected) { view.startButton.setText("Stop"); diff --git a/src/main/java/com/easternedgerobotics/rov/io/TcpFileReceiver.java b/src/main/java/com/easternedgerobotics/rov/io/TcpFileReceiver.java new file mode 100644 index 0000000..3e335f6 --- /dev/null +++ b/src/main/java/com/easternedgerobotics/rov/io/TcpFileReceiver.java @@ -0,0 +1,132 @@ +package com.easternedgerobotics.rov.io; + +import org.pmw.tinylog.Logger; +import rx.Observable; +import rx.Observer; +import rx.observables.SyncOnSubscribe; +import rx.schedulers.Schedulers; +import rx.subscriptions.CompositeSubscription; + +import java.io.BufferedReader; +import java.io.File; +import java.io.IOException; +import java.io.InputStream; +import java.io.InputStreamReader; +import java.net.ServerSocket; +import java.net.Socket; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.nio.file.StandardCopyOption; + +public final class TcpFileReceiver { + /** + * Contain the subscription for the server observable. + */ + private final CompositeSubscription subscription = new CompositeSubscription(); + + /** + * the socket backlog variable to be used in the server client listener. + */ + private final int socketBacklog; + + /** + * the port used to create the server client listener. + */ + private final int port; + + /** + * Run a server which will accept client connections and download files. + * All connections begin with a string file path followed by the file content. + * The file content will be saved to that file path. + * + * @param port the port to start the file receiver on. + * @param socketBacklog the socket backlog for the server. + */ + public TcpFileReceiver( + final int port, + final int socketBacklog + ) { + this.port = port; + this.socketBacklog = socketBacklog; + } + + /** + * Begin downloading files from remote connections. + */ + public void start() { + final Observable source = Observable.create(new TcpFileReceiverSyncOnSubscribe()); + subscription.add(source.subscribeOn(Schedulers.newThread()).subscribe(Logger::info, Logger::error)); + } + + /** + * Stop downloading files. + */ + public void stop() { + subscription.clear(); + } + + private final class TcpFileReceiverSyncOnSubscribe extends SyncOnSubscribe { + /** + * Create the initial server connection for the observable. + * + * @return the receiver server + */ + @Override + protected ServerSocket generateState() { + try { + return new ServerSocket(port, socketBacklog); + } catch (final IOException e) { + throw new RuntimeException(e); + } + } + + /** + * Grab a client and download their file. + * + * @param server the receiver server. + * @param observer the listener to this receiver. + * @return the server + */ + @Override + protected ServerSocket next(final ServerSocket server, final Observer observer) { + if (server.isClosed()) { + observer.onCompleted(); + return server; + } + try { + try (final Socket clientSocket = server.accept(); + final InputStream inputStream = clientSocket.getInputStream(); + final BufferedReader reader = new BufferedReader(new InputStreamReader(inputStream)); + ) { + final String pathName = reader.readLine(); + final Path outputPath = Paths.get(pathName); + // Dirty hack to avoid the fact that streaming to a new file is not an atomic operation. + // The file name is created then the data is loaded to it. This causes services such as + // WatchService to see the file before it is created. The tmp file copy is a lot faster + // than the tcp copy, so this prevents reading an incomplete outputPath file. + final Path temp = File.createTempFile("TcpFileReceiver", ".tmp").toPath(); + Files.copy(inputStream, temp, StandardCopyOption.REPLACE_EXISTING); + Files.copy(temp, outputPath, StandardCopyOption.REPLACE_EXISTING); + observer.onNext(outputPath); + } + } catch (final IOException e) { + observer.onError(e); + } + return server; + } + + /** + * When the object is unsubscribed, close the server connection. + * + * @param server the server associated with this instance. + */ + protected void onUnsubscribe(final ServerSocket server) { + try { + server.close(); + } catch (final IOException e) { + throw new RuntimeException(e); + } + } + } +} diff --git a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java index 277a591..9ce575a 100644 --- a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java +++ b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java @@ -1,13 +1,16 @@ package com.easternedgerobotics.rov.video; import com.easternedgerobotics.rov.config.CameraCalibrationConfig; +import com.easternedgerobotics.rov.event.EventPublisher; +import com.easternedgerobotics.rov.io.DirectoryUtil; import com.easternedgerobotics.rov.io.FileUtil; +import com.easternedgerobotics.rov.value.CameraCaptureValueA; +import com.easternedgerobotics.rov.value.CameraCaptureValueB; -import org.pmw.tinylog.Logger; import rx.Scheduler; import java.io.File; -import java.io.IOException; +import java.nio.file.Paths; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicReference; @@ -33,9 +36,9 @@ public class CameraCalibration { private final AtomicReference correctorB = new AtomicReference<>(); /** - * The source of the video frames to be calibrated. + * The network event publisher to communicate with the cameras. */ - private final VideoDecoder videoDecoder; + private final EventPublisher eventPublisher; /** * The calibration settings. @@ -50,16 +53,16 @@ public class CameraCalibration { /** * Create a calibration object which can perform a chessboard calibration. * - * @param videoDecoder The source of the video frames to be calibrated. - * @param config The calibration settings. - * @param scheduler The scheduler used when performing calibration work. + * @param eventPublisher The network event publisher to communicate with the cameras. + * @param config The calibration settings. + * @param scheduler The scheduler used when performing calibration work. */ public CameraCalibration( - final VideoDecoder videoDecoder, + final EventPublisher eventPublisher, final CameraCalibrationConfig config, final Scheduler scheduler ) { - this.videoDecoder = videoDecoder; + this.eventPublisher = eventPublisher; this.config = config; this.scheduler = scheduler; scheduler.createWorker().schedule(() -> @@ -98,33 +101,29 @@ public void calibrate() { /** * Capture an image to be used with the camera A calibration. - * Timeout for 1 second in case the stream is not initialized. + * Timeout for 10 second in case the stream is not initialized. */ public void captureCalibrationImageA() { - videoDecoder.cameraAImages().observeOn(scheduler).timeout(1, TimeUnit.SECONDS).take(1).subscribe(image -> { - final File outputFile = FileUtil.nextName(config.cameraAImagesDirectory(), "checkerboardA", "png"); - if (outputFile != null) { - FileUtil.saveImageFile(image, outputFile, "png"); - } - }); + final File destination = FileUtil.nextName(config.cameraAImagesDirectory(), "checkerboardA", "png"); + if (destination != null) { + eventPublisher.emit(new CameraCaptureValueA(destination.getAbsolutePath())); + } } /** * Capture an image to be used with the camera B calibration. - * Timeout for 1 second in case the stream is not initialized. + * Timeout for 10 second in case the stream is not initialized. */ public void captureCalibrationImageB() { - videoDecoder.cameraBImages().observeOn(scheduler).timeout(1, TimeUnit.SECONDS).take(1).subscribe(image -> { - final File outputFile = FileUtil.nextName(config.cameraBImagesDirectory(), "checkerboardB", "png"); - if (outputFile != null) { - FileUtil.saveImageFile(image, outputFile, "png"); - } - }); + final File destination = FileUtil.nextName(config.cameraBImagesDirectory(), "checkerboardB", "png"); + if (destination != null) { + eventPublisher.emit(new CameraCaptureValueB(destination.getAbsolutePath())); + } } /** * Capture an image to be used with the camera A calibration. - * Timeout for 1 second in case the stream is not initialized. + * Timeout for 1 minute in case the stream is not initialized. * * @param saveFile the output destination */ @@ -136,21 +135,21 @@ public void captureUndistortedImageA(final File saveFile) { } correctorA.set(new DistortionCorrector(result)); } - final DistortionCorrector corrector = correctorA.get(); - videoDecoder.cameraAImages().observeOn(scheduler).timeout(1, TimeUnit.SECONDS).take(1).subscribe(image -> { - try { - final File tempFile = File.createTempFile(saveFile.getName(), ".tmp"); - FileUtil.saveImageFile(image, tempFile, "png"); - corrector.apply(tempFile, saveFile); - } catch (final IOException e) { - Logger.error(e); - } - }); + final File destination = FileUtil.nextName(config.cameraAPreUndistortedDirectory(), "captureA", "png"); + if (destination != null) { + final DistortionCorrector corrector = correctorA.get(); + DirectoryUtil.observe(Paths.get(config.cameraAPreUndistortedDirectory())) + .filter(destination.toPath()::equals).take(1) + .timeout(1, TimeUnit.MINUTES) + .observeOn(scheduler) + .subscribe(path -> corrector.apply(destination, saveFile)); + eventPublisher.emit(new CameraCaptureValueA(destination.getAbsolutePath())); + } } /** * Capture an image to be used with the camera B calibration. - * Timeout for 1 second in case the stream is not initialized. + * Timeout for 1 minute in case the stream is not initialized. * * @param saveFile the output destination */ @@ -162,15 +161,15 @@ public void captureUndistortedImageB(final File saveFile) { } correctorB.set(new DistortionCorrector(result)); } - final DistortionCorrector corrector = correctorB.get(); - videoDecoder.cameraBImages().observeOn(scheduler).timeout(1, TimeUnit.SECONDS).take(1).subscribe(image -> { - try { - final File tempFile = File.createTempFile(saveFile.getName(), ".tmp"); - FileUtil.saveImageFile(image, tempFile, "png"); - corrector.apply(tempFile, saveFile); - } catch (final IOException e) { - Logger.error(e); - } - }); + final File destination = FileUtil.nextName(config.cameraBPreUndistortedDirectory(), "captureB", "png"); + if (destination != null) { + final DistortionCorrector corrector = correctorB.get(); + DirectoryUtil.observe(Paths.get(config.cameraBPreUndistortedDirectory())) + .filter(destination.toPath()::equals).take(1) + .timeout(1, TimeUnit.MINUTES) + .observeOn(scheduler) + .subscribe(path -> corrector.apply(destination, saveFile)); + eventPublisher.emit(new CameraCaptureValueB(destination.getAbsolutePath())); + } } } diff --git a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibrationResult.java b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibrationResult.java index f4fa7f6..8322438 100644 --- a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibrationResult.java +++ b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibrationResult.java @@ -1,5 +1,6 @@ package com.easternedgerobotics.rov.video; +import org.bytedeco.javacpp.indexer.DoubleIndexer; import org.bytedeco.javacpp.opencv_core.Mat; import org.bytedeco.javacpp.opencv_core.Size; @@ -30,12 +31,30 @@ public final class CameraCalibrationResult { */ private final Mat cameraMatrix; + private final double fx; + + private final double fy; + + private final double cx; + + private final double cy; + /** * This distortion coefficients associated with the camera. * Turns real world coordinates into image space coordinates. */ private final Mat distortionCoeffs; + private final double k1; + + private final double k2; + + private final double p1; + + private final double p2; + + private final double k3; + /** * The results from running a camera calibration. * @@ -45,6 +64,7 @@ public final class CameraCalibrationResult { * @param cameraMatrix This intrinsic camera matrix. * @param distortionCoeffs The distortion coefficients associated with the camera. */ + @SuppressWarnings({"checkstyle:magicnumber"}) public CameraCalibrationResult( final List validFileNames, final Size imageSize, @@ -57,6 +77,29 @@ public CameraCalibrationResult( this.calibrateCameraRmsError = calibrateCameraRmsError; this.cameraMatrix = cameraMatrix; this.distortionCoeffs = distortionCoeffs; + final DoubleIndexer cameraMatrixIndexer = cameraMatrix.createIndexer(); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + System.out.print(cameraMatrixIndexer.get(i, j) + " "); + } + System.out.println(); + } + fx = cameraMatrixIndexer.get(0, 0); + fy = cameraMatrixIndexer.get(1, 1); + cx = cameraMatrixIndexer.get(0, 2); + cy = cameraMatrixIndexer.get(1, 2); + final DoubleIndexer distortionCoeffsIndexer = distortionCoeffs.createIndexer(); + for (int i = 0; i < 1; i++) { + for (int j = 0; j < 5; j++) { + System.out.print(distortionCoeffsIndexer.get(i, j) + " "); + } + System.out.println(); + } + k1 = distortionCoeffsIndexer.get(0, 0); + k2 = distortionCoeffsIndexer.get(0, 1); + p1 = distortionCoeffsIndexer.get(0, 2); + p2 = distortionCoeffsIndexer.get(0, 3); + k3 = distortionCoeffsIndexer.get(0, 4); } /** diff --git a/src/main/java/com/easternedgerobotics/rov/video/PicameraVideo.java b/src/main/java/com/easternedgerobotics/rov/video/PicameraVideo.java index a6543af..2a2d306 100644 --- a/src/main/java/com/easternedgerobotics/rov/video/PicameraVideo.java +++ b/src/main/java/com/easternedgerobotics/rov/video/PicameraVideo.java @@ -30,10 +30,10 @@ public static void stop() { } } - public static void start(final String host, final int port) { + public static void start(final String host, final int videoPort, final int imagePort) { Logger.info("Starting video"); try (final PrintWriter out = new PrintWriter("/run/eer/camera-environment")) { - out.println(String.format("host=%s\nport=%s", host, port)); + out.println(String.format("host=%s\nvideo_port=%s\nimage_port=%s", host, videoPort, imagePort)); out.flush(); out.close(); RUNTIME.exec(new String[] {"systemctl", "restart", SERVICE}); @@ -50,4 +50,16 @@ public static void flip() { Logger.error(e); } } + + public static void takeImage(final String path) { + Logger.info("Taking image and writing to path: " + path); + try (final PrintWriter out = new PrintWriter("/run/eer/camera-image-path")) { + out.println(path); + out.flush(); + out.close(); + RUNTIME.exec(new String[] {"systemctl", "kill", SERVICE, "--signal=SIGUSR2"}); + } catch (final IOException e) { + Logger.error(e); + } + } } diff --git a/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueA.kt b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueA.kt new file mode 100644 index 0000000..e3012a6 --- /dev/null +++ b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueA.kt @@ -0,0 +1,3 @@ +package com.easternedgerobotics.rov.value + +class CameraCaptureValueA(val path: String = "tmp") diff --git a/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueB.kt b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueB.kt new file mode 100644 index 0000000..d0b2b47 --- /dev/null +++ b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCaptureValueB.kt @@ -0,0 +1,3 @@ +package com.easternedgerobotics.rov.value + +class CameraCaptureValueB(val path: String = "tmp") diff --git a/src/test/java/com/easternedgerobotics/rov/config/MockLaunchConfig.java b/src/test/java/com/easternedgerobotics/rov/config/MockLaunchConfig.java index 91f28cf..5657b53 100644 --- a/src/test/java/com/easternedgerobotics/rov/config/MockLaunchConfig.java +++ b/src/test/java/com/easternedgerobotics/rov/config/MockLaunchConfig.java @@ -22,4 +22,14 @@ public int baudRate() { public int heartbeatRate() { return 1; } + + @Override + public int fileReceiverPort() { + return 12347; + } + + @Override + public int fileReceiverSocketBacklog() { + return 100; + } }