From 951aa1bdbdb931af3fb7b634283685b6dc2b5426 Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Thu, 26 Mar 2020 22:03:27 -0400 Subject: [PATCH] Add all relevant 2.X files to _2 package, move/port some 2.X classes --- chameleon-server/build.gradle | 52 +- chameleon-server/settings.gradle | 2 +- .../java/com/chameleonvision/_2/Main.java | 181 +++++++ .../_2/config/CameraCalibrationConfig.java | 64 +++ .../_2/config/CameraConfig.java | 181 +++++++ .../_2/config/CameraJsonConfig.java | 53 +++ .../_2/config/ConfigManager.java | 126 +++++ .../_2/config/FullCameraConfiguration.java | 21 + .../_2/config/GeneralSettings.java | 14 + .../chameleonvision/_2/config/JsonMat.java | 74 +++ .../_2/config/PipelineConfig.java | 144 ++++++ .../config/serializers/BaseDeserializer.java | 130 +++++ .../_2/config/serializers/BaseSerializer.java | 49 ++ ...tandardCVPipelineSettingsDeserializer.java | 79 ++++ .../StandardCVPipelineSettingsSerializer.java | 82 ++++ .../_2/network/LinuxNetworking.java | 103 ++++ .../_2/network/NetmaskToCIDR.java | 29 ++ .../_2/network/NetworkInterface.java | 54 +++ .../_2/network/NetworkManager.java | 106 +++++ .../_2/network/SysNetworking.java | 36 ++ .../_2/network/WindowsNetworking.java | 54 +++ .../com/chameleonvision/_2/util/Helpers.java | 54 +++ .../_2/util/ProgramDirectoryUtilities.java | 37 ++ .../_2/vision/VisionManager.java | 187 ++++++++ .../_2/vision/VisionProcess.java | 389 +++++++++++++++ .../_2/vision/camera/CameraCapture.java | 48 ++ .../_2/vision/camera/CameraStreamer.java | 103 ++++ .../camera/CaptureStaticProperties.java | 39 ++ .../_2/vision/camera/USBCameraCapture.java | 142 ++++++ .../vision/camera/USBCaptureProperties.java | 117 +++++ .../_2/vision/enums/CalibrationMode.java | 5 + .../_2/vision/enums/ImageFlipMode.java | 14 + .../_2/vision/enums/ImageRotationMode.java | 16 + .../_2/vision/enums/SortMode.java | 5 + .../_2/vision/enums/StreamDivisor.java | 14 + .../_2/vision/enums/TargetGroup.java | 6 + .../_2/vision/enums/TargetIntersection.java | 5 + .../_2/vision/enums/TargetOrientation.java | 5 + .../_2/vision/enums/TargetRegion.java | 5 + .../_2/vision/image/CaptureProperties.java | 30 ++ .../_2/vision/image/ImageCapture.java | 12 + .../_2/vision/image/StaticImageCapture.java | 87 ++++ .../_2/vision/pipeline/CVPipeline.java | 32 ++ .../_2/vision/pipeline/CVPipelineResult.java | 26 + .../vision/pipeline/CVPipelineSettings.java | 18 + .../_2/vision/pipeline/Pipe.java | 13 + .../_2/vision/pipeline/PipelineManager.java | 251 ++++++++++ .../pipeline/impl/Calibrate3dPipeline.java | 170 +++++++ .../pipeline/impl/DriverVisionPipeline.java | 55 +++ .../pipeline/impl/StandardCVPipeline.java | 284 +++++++++++ .../impl/StandardCVPipelineSettings.java | 49 ++ .../_2/vision/pipeline/pipes/BlurPipe.java | 42 ++ .../pipeline/pipes/Collect2dTargetsPipe.java | 128 +++++ .../pipeline/pipes/Draw2dContoursPipe.java | 104 ++++ .../pipeline/pipes/Draw2dCrosshairPipe.java | 85 ++++ .../pipeline/pipes/DrawSolvePNPPipe.java | 126 +++++ .../pipeline/pipes/ErodeDilatePipe.java | 51 ++ .../pipeline/pipes/FilterContoursPipe.java | 77 +++ .../pipeline/pipes/FindContoursPipe.java | 29 ++ .../pipeline/pipes/GroupContoursPipe.java | 195 ++++++++ .../_2/vision/pipeline/pipes/HsvPipe.java | 46 ++ .../vision/pipeline/pipes/OutputMatPipe.java | 50 ++ .../vision/pipeline/pipes/RotateFlipPipe.java | 55 +++ .../vision/pipeline/pipes/SolvePNPPipe.java | 443 ++++++++++++++++++ .../pipeline/pipes/SortContoursPipe.java | 93 ++++ .../pipeline/pipes/SpeckleRejectPipe.java | 54 +++ .../_2/web/RequestHandler.java | 251 ++++++++++ .../com/chameleonvision/_2/web/Server.java | 41 ++ .../chameleonvision/_2/web/SocketHandler.java | 306 ++++++++++++ .../common/configuration/ConfigManager.java | 5 - .../common/datatransfer/DataConsumer.java | 4 + .../common/datatransfer/DataProvider.java | 4 + .../networktables/NetworkTablesManager.java | 75 +++ .../common/logging/DebugLogger.java | 21 + .../LinuxNetworking.java | 2 +- .../NetworkInterface.java | 2 +- .../NetworkManager.java | 2 +- .../{network => networking}/NetworkMode.java | 2 +- .../SysNetworking.java | 4 +- .../common/scripting/ScriptCommandType.java | 14 + .../common/scripting/ScriptConfig.java | 23 + .../common/scripting/ScriptEvent.java | 33 ++ .../common/scripting/ScriptEventType.java | 21 + .../common/scripting/ScriptManager.java | 126 +++++ .../server/configuration/MainConfig.java | 21 - .../server/configuration/NetworkConfig.java | 23 - .../common/server/util/ShellExecutor.java | 45 -- .../common/util/ColorHelper.java | 11 + .../common/util/LoopingRunnable.java | 37 ++ .../common/util/MemoryManager.java | 66 +++ .../common/{server => }/util/Platform.java | 2 +- .../common/{server => }/util/ShellExec.java | 2 +- .../common/util/file/FileUtils.java | 51 ++ .../common/util/file/JacksonUtils.java | 74 +++ .../common/util/math/IPUtils.java | 38 ++ .../common/util/math/MathUtils.java | 30 ++ .../common/util/numbers/DoubleCouple.java | 12 + .../common/util/numbers/IntegerCouple.java | 12 + .../common/util/numbers/NumberCouple.java | 33 ++ .../common/vision/base/camera/USBCamera.java | 2 +- .../vision/base/frame/FrameConsumer.java | 2 +- .../frame/consumer/MJPGFrameConsumer.java | 12 + .../provider}/FileFrameProvider.java | 2 +- .../provider}/NetworkFrameProvider.java | 2 +- .../provider}/USBFrameProvider.java | 2 +- .../base/pipeline/{Pipe.java => CVPipe.java} | 9 +- .../vision/base/pipeline/DummyPipeline.java | 35 ++ .../base/pipeline/pipe/ResizeImagePipe.java | 23 +- .../base/pipeline/pipe/RotateImagePipe.java | 31 ++ .../pipe/javacv/GPUResizeImagePipe.java | 16 - .../pipe/params/ResizeImageParams.java | 26 + .../pipe/params/RotateImageParams.java | 33 ++ .../vision/base/stream/AsyncMjpgStreamer.java | 4 - .../vision/base/stream/MjpgStreamer.java | 4 - .../common/vision/base/stream/Streamer.java | 4 - .../common/vision/opencv/OpenCVWrapper.java | 4 - 116 files changed, 6862 insertions(+), 172 deletions(-) create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/Main.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsSerializer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/LinuxNetworking.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageFlipMode.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/StreamDivisor.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetGroup.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineResult.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineSettings.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/DriverVisionPipeline.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dContoursPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dCrosshairPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/DrawSolvePNPPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/ErodeDilatePipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SpeckleRejectPipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java rename chameleon-server/src/main/java/com/chameleonvision/common/{network => networking}/LinuxNetworking.java (98%) rename chameleon-server/src/main/java/com/chameleonvision/common/{network => networking}/NetworkInterface.java (97%) rename chameleon-server/src/main/java/com/chameleonvision/common/{network => networking}/NetworkManager.java (91%) rename chameleon-server/src/main/java/com/chameleonvision/common/{network => networking}/NetworkMode.java (51%) rename chameleon-server/src/main/java/com/chameleonvision/common/{network => networking}/SysNetworking.java (95%) create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/MainConfig.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/NetworkConfig.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExecutor.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/ColorHelper.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/LoopingRunnable.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/MemoryManager.java rename chameleon-server/src/main/java/com/chameleonvision/common/{server => }/util/Platform.java (98%) rename chameleon-server/src/main/java/com/chameleonvision/common/{server => }/util/ShellExec.java (99%) create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/file/JacksonUtils.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/math/IPUtils.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/IntegerCouple.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/consumer/MJPGFrameConsumer.java rename chameleon-server/src/main/java/com/chameleonvision/common/vision/base/{capture => frame/provider}/FileFrameProvider.java (84%) rename chameleon-server/src/main/java/com/chameleonvision/common/vision/base/{capture => frame/provider}/NetworkFrameProvider.java (84%) rename chameleon-server/src/main/java/com/chameleonvision/common/vision/base/{capture => frame/provider}/USBFrameProvider.java (84%) rename chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/{Pipe.java => CVPipe.java} (77%) create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/DummyPipeline.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/RotateImagePipe.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/javacv/GPUResizeImagePipe.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/ResizeImageParams.java create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/RotateImageParams.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/AsyncMjpgStreamer.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/MjpgStreamer.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/Streamer.java delete mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/OpenCVWrapper.java diff --git a/chameleon-server/build.gradle b/chameleon-server/build.gradle index e25b530a6..73e6cb56f 100644 --- a/chameleon-server/build.gradle +++ b/chameleon-server/build.gradle @@ -44,32 +44,32 @@ dependencies { implementation "com.moandjiezana.toml:toml4j:0.7.2" - // javacv - def withoutJunk = { - exclude group: 'org.bytedeco', module: 'artoolkitplus' - exclude group: 'org.bytedeco', module: 'artoolkitplus-platform' - exclude group: 'org.bytedeco', module: 'flandmark' - exclude group: 'org.bytedeco', module: 'flandmark-platform' - exclude group: 'org.bytedeco', module: 'flycapture' - exclude group: 'org.bytedeco', module: 'flycapture-platform' - exclude group: 'org.bytedeco', module: 'leptonica' - exclude group: 'org.bytedeco', module: 'leptonica-platform' - exclude group: 'org.bytedeco', module: 'libdc1394' - exclude group: 'org.bytedeco', module: 'libdc1394-platform' - exclude group: 'org.bytedeco', module: 'libfreenect' - exclude group: 'org.bytedeco', module: 'libfreenect-platform' - exclude group: 'org.bytedeco', module: 'libfreenect2' - exclude group: 'org.bytedeco', module: 'libfreenect2-platform' - exclude group: 'org.bytedeco', module: 'librealsense' - exclude group: 'org.bytedeco', module: 'librealsense-platform' - exclude group: 'org.bytedeco', module: 'librealsense2' - exclude group: 'org.bytedeco', module: 'librealsense2-platform' - exclude group: 'org.bytedeco', module: 'openblas' - exclude group: 'org.bytedeco', module: 'openblas-platform' - exclude group: 'org.bytedeco', module: 'tesseract' - exclude group: 'org.bytedeco', module: 'tesseract-platform' - } - compile 'org.bytedeco:javacv-platform:1.5.2', withoutJunk +// // javacv +// def withoutJunk = { +// exclude group: 'org.bytedeco', module: 'artoolkitplus' +// exclude group: 'org.bytedeco', module: 'artoolkitplus-platform' +// exclude group: 'org.bytedeco', module: 'flandmark' +// exclude group: 'org.bytedeco', module: 'flandmark-platform' +// exclude group: 'org.bytedeco', module: 'flycapture' +// exclude group: 'org.bytedeco', module: 'flycapture-platform' +// exclude group: 'org.bytedeco', module: 'leptonica' +// exclude group: 'org.bytedeco', module: 'leptonica-platform' +// exclude group: 'org.bytedeco', module: 'libdc1394' +// exclude group: 'org.bytedeco', module: 'libdc1394-platform' +// exclude group: 'org.bytedeco', module: 'libfreenect' +// exclude group: 'org.bytedeco', module: 'libfreenect-platform' +// exclude group: 'org.bytedeco', module: 'libfreenect2' +// exclude group: 'org.bytedeco', module: 'libfreenect2-platform' +// exclude group: 'org.bytedeco', module: 'librealsense' +// exclude group: 'org.bytedeco', module: 'librealsense-platform' +// exclude group: 'org.bytedeco', module: 'librealsense2' +// exclude group: 'org.bytedeco', module: 'librealsense2-platform' +// exclude group: 'org.bytedeco', module: 'openblas' +// exclude group: 'org.bytedeco', module: 'openblas-platform' +// exclude group: 'org.bytedeco', module: 'tesseract' +// exclude group: 'org.bytedeco', module: 'tesseract-platform' +// } +// compile 'org.bytedeco:javacv-platform:1.5.2', withoutJunk // wpilib stuff implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion" diff --git a/chameleon-server/settings.gradle b/chameleon-server/settings.gradle index c1d0ca741..ac2805e99 100644 --- a/chameleon-server/settings.gradle +++ b/chameleon-server/settings.gradle @@ -1,2 +1,2 @@ -rootProject.name = 'ChameleonVision' +rootProject.name = 'ChameleonServer' diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java b/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java new file mode 100644 index 000000000..6719f055d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java @@ -0,0 +1,181 @@ +package com.chameleonvision._2; + +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.vision.VisionManager; +import com.chameleonvision._2.web.Server; +import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager; +import com.chameleonvision.common.networking.NetworkManager; +import com.chameleonvision.common.scripting.ScriptEventType; +import com.chameleonvision.common.scripting.ScriptManager; +import com.chameleonvision.common.util.Platform; +import com.chameleonvision.common.util.math.IPUtils; +import edu.wpi.cscore.CameraServerCvJNI; +import edu.wpi.cscore.CameraServerJNI; + +import java.io.IOException; + +import static com.chameleonvision.common.util.Platform.CurrentPlatform; + +public class Main { + + private static final String NT_SERVERMODE_KEY = "--nt-servermode"; // no args for this setting + private static final String NT_CLIENTMODESERVER_KEY = "--nt-client-server"; // expects String representing an IP address (hostnames will be rejected!) + private static final String NETWORK_MANAGE_KEY = "--unmanage-network"; // no args for this setting + private static final String IGNORE_ROOT_KEY = "--ignore-root"; // no args for this setting + private static final String TEST_MODE_KEY = "--cv-development"; + private static final String UI_PORT_KEY = "--ui-port"; + + private static final int DEFAULT_PORT = 5800; + + private static boolean ntServerMode = false; + private static boolean manageNetwork = true; + private static boolean ignoreRoot = false; + private static String ntClientModeServer = null; + public static boolean testMode = false; + public static int uiPort = DEFAULT_PORT; + + private static void handleArgs(String[] args) { + for (int i = 0; i < args.length; i++) { + var key = args[i].toLowerCase(); + String value = null; + + // this switch handles arguments with a value. Add any settings with a value here. + switch (key) { + case NT_CLIENTMODESERVER_KEY: + var potentialValue = args[i + 1]; + // ensures this "value" isnt null, blank, nor another argument + if (potentialValue != null && !potentialValue.isBlank() && !potentialValue.startsWith("-") & !potentialValue.startsWith("--")) { + value = potentialValue.toLowerCase(); + } + i++; // increment to skip an 'arg' next go-around of for loop, as that would be this value + break; + case UI_PORT_KEY: + var potentialPort = args[i + 1]; + if (potentialPort != null && !potentialPort.isBlank() && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) { + value = potentialPort; + } + i++; + break; + case NT_SERVERMODE_KEY: + case NETWORK_MANAGE_KEY: + case IGNORE_ROOT_KEY: + case TEST_MODE_KEY: + // nothing + break; + } + + // this switch actually handles the arguments. + switch (key) { + case NT_SERVERMODE_KEY: + ntServerMode = true; + break; + case NT_CLIENTMODESERVER_KEY: + if (value != null) { + if (value.equals("localhost")) { + ntClientModeServer = "127.0.0.1"; + continue; + } + + if (IPUtils.isValidIPV4(value)) { + ntClientModeServer = value; + continue; + } + } + System.err.println("Argument for NT Server Host was invalid, defaulting to team number host"); + break; + case NETWORK_MANAGE_KEY: + manageNetwork = false; + break; + case IGNORE_ROOT_KEY: + ignoreRoot = true; + break; + case TEST_MODE_KEY: + testMode = true; + break; + case UI_PORT_KEY: + if (value != null) { + try { + uiPort = Integer.parseInt(value); + } catch (NumberFormatException e){ + System.err.println("ui Port was not a valid number using port 5800"); + } + } + break; + } + } + } + + public static void main(String[] args) { + + Runtime.getRuntime().addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit))); + + if (CurrentPlatform.equals(Platform.UNSUPPORTED)) { + System.err.printf("Sorry, this platform is not supported. Give these details to the developers.\n%s\n", CurrentPlatform.toString()); + return; + } else { + System.out.printf("Starting Chameleon Vision on platform %s\n", CurrentPlatform.toString()); + } + + handleArgs(args); + + if (!CurrentPlatform.isRoot) { + if (ignoreRoot) { + // manageNetwork = false; + System.out.println("Ignoring root, network will not be managed!"); + } else { + System.err.println("This program must be run as root!"); + return; + } + } + + // Attempt to load the JNI Libraries + System.out.println("Loading CameraServer..."); + try { + CameraServerJNI.forceLoad(); + CameraServerCvJNI.forceLoad(); + } catch (UnsatisfiedLinkError | IOException e) { + if (CurrentPlatform.isWindows()) { + System.err.println("Try to download the VC++ Redistributable, https://aka.ms/vs/16/release/vc_redist.x64.exe"); + } + throw new RuntimeException("Failed to load JNI Libraries!"); + } + + System.out.println("Checking Settings..."); + ConfigManager.initializeSettings(); + + if (!CurrentPlatform.isWindows()) { + System.out.println("Initializing Script Manager..."); + ScriptManager.initialize(); + } else { + System.out.println("Scripts not yet supported on Windows. ScriptEvents will be ignored."); + } + + NetworkManager.getInstance().initialize(manageNetwork); + + if (ntServerMode) { + NetworkTablesManager.setServerMode(); + } else { + NetworkTablesManager.setClientMode(ntClientModeServer); + } + + ScriptManager.queueEvent(ScriptEventType.kProgramInit); + + boolean visionSourcesOk = VisionManager.initializeSources(); + if (!visionSourcesOk) { + System.err.println("No cameras connected!"); + return; + } + + boolean visionProcessesOk = VisionManager.initializeProcesses(); + if (!visionProcessesOk) { + System.err.println("Failed to initialize vision processes!"); + return; + } + + System.out.println("Starting vision processes..."); + VisionManager.startProcesses(); + + System.out.printf("Starting Web server at port %d\n", uiPort); + Server.main(uiPort); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java new file mode 100644 index 000000000..c2326d1ea --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java @@ -0,0 +1,64 @@ +package com.chameleonvision._2.config; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnore; +import com.fasterxml.jackson.annotation.JsonIgnoreType; +import com.fasterxml.jackson.annotation.JsonProperty; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.Size; + +/** + * A class that holds a camera matrix and distortion coefficients for a given resolution + */ +public class CameraCalibrationConfig { + @JsonProperty("resolution") public final Size resolution; + @JsonProperty("cameraMatrix") public final JsonMat cameraMatrix; + @JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs; + @JsonProperty("squareSize") public final double squareSize; + + @JsonCreator + public CameraCalibrationConfig( + @JsonProperty("resolution") Size resolution, + @JsonProperty("cameraMatrix") JsonMat cameraMatrix, + @JsonProperty("distortionCoeffs") JsonMat distortionCoeffs, + @JsonProperty("squareSize") double squareSize) { + this.resolution = resolution; + this.cameraMatrix = cameraMatrix; + this.distortionCoeffs = distortionCoeffs; + this.squareSize = squareSize; + } + + public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) { + this.resolution = resolution; + this.cameraMatrix = JsonMat.fromMat(cameraMatrix); + this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs); + this.squareSize = squareSize; + } + + @JsonIgnoreType + public static class UICameraCalibrationConfig { + public final int width; + public final int height; + public final double[] cameraMatrix; + public final double[] distortionCoeffs; + + public UICameraCalibrationConfig(CameraCalibrationConfig config) { + width = (int) config.resolution.width; + height = (int) config.resolution.height; + cameraMatrix = config.cameraMatrix.data; + distortionCoeffs = config.distortionCoeffs.data; + } + + } + + @JsonIgnore + public Mat getCameraMatrixAsMat() { + return cameraMatrix.toMat(); + } + + @JsonIgnore + public MatOfDouble getDistortionCoeffsAsMat() { + return new MatOfDouble(distortionCoeffs.toMat()); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java new file mode 100644 index 000000000..21586c33d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java @@ -0,0 +1,181 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision.common.util.file.FileUtils; +import com.chameleonvision.common.util.file.JacksonUtils; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; + +public class CameraConfig { + + private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras"); + + private final CameraJsonConfig preliminaryConfig; + private final Path configFolderPath; + private final Path configPath; + private final Path driverModePath; + private final Path calibrationPath; + final Path pipelineFolderPath; + + public final PipelineConfig pipelineConfig; + + CameraConfig(CameraJsonConfig config) { + preliminaryConfig = config; + String cameraConfigName = preliminaryConfig.name.replace(' ', '_'); + pipelineConfig = new PipelineConfig(this); + + configFolderPath = Path.of(camerasConfigFolderPath.toString(), cameraConfigName); + configPath = Path.of(configFolderPath.toString(), "camera.json"); + driverModePath = Path.of(configFolderPath.toString(), "drivermode.json"); + calibrationPath = Path.of(configFolderPath.toString(), "calibration.json"); + pipelineFolderPath = Paths.get(configFolderPath.toString(), "pipelines"); + } + + public FullCameraConfiguration load() { + checkFolder(); + checkConfig(); + checkDriverMode(); + checkCalibration(); + pipelineConfig.check(); + + return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this); + } + + private CameraJsonConfig loadConfig() { + CameraJsonConfig config = preliminaryConfig; + try { + config = JacksonUtils.deserialize(configPath, CameraJsonConfig.class); + } catch (IOException e) { + System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString()); + } + return config; + } + + private CVPipelineSettings loadDriverMode() { + CVPipelineSettings driverMode = new CVPipelineSettings(); + try { + driverMode = JacksonUtils.deserialize(driverModePath, CVPipelineSettings.class); + } catch (IOException e) { + System.err.println("Failed to load camera drivermode: " + driverModePath.toString()); + } + if (driverMode != null) { + driverMode.nickname = "DRIVERMODE"; + driverMode.index = -1; + } + return driverMode; + } + + private List loadCalibration() { + List calibrations = new ArrayList<>(); + try { + calibrations = List.of(Objects.requireNonNull(JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class))); + } catch (Exception e) { + System.err.println("Failed to load camera calibration: " + driverModePath.toString()); + } + return calibrations; + } + + void saveConfig(CameraJsonConfig config) { + try { + JacksonUtils.serializer(configPath, config, true); + FileUtils.setFilePerms(configPath); + } catch (IOException e) { + System.err.println("Failed to save camera config file: " + configPath.toString()); + } + } + + void savePipelines(List pipelines) { + pipelineConfig.save(pipelines); + } + + public void saveDriverMode(CVPipelineSettings driverMode) { + try { + JacksonUtils.serializer(driverModePath, driverMode, true); + FileUtils.setFilePerms(driverModePath); + } catch (IOException e) { + System.err.println("Failed to save camera drivermode file: " + driverModePath.toString()); + } + } + + + public void saveCalibration(List cal) { + CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]); + try { + JacksonUtils.serializer(calibrationPath, configs, true); + FileUtils.setFilePerms(calibrationPath); + } catch (IOException e) { + System.err.println("Failed to save camera calibration file: " + calibrationPath.toString()); + } + } + + void checkFolder() { + if (!configFolderExists()) { + try { + if (!(new File(configFolderPath.toUri()).mkdirs())) { + System.err.println("Failed to create camera config folder: " + configFolderPath.toString()); + } + FileUtils.setFilePerms(configFolderPath); + } catch (Exception e) { + System.err.println("Failed to create camera config folder: " + configFolderPath.toString()); + } + } + } + + private void checkConfig() { + if (!configExists()) { + try { + JacksonUtils.serializer(configPath, preliminaryConfig, true); + FileUtils.setFilePerms(configPath); + } catch (IOException e) { + System.err.println("Failed to create camera config file: " + configPath.toString()); + } + } + } + + private void checkDriverMode() { + if (!driverModeExists()) { + try { + CVPipelineSettings newDriverModeSettings = new CVPipelineSettings(); + newDriverModeSettings.nickname = "DRIVERMODE"; + JacksonUtils.serializer(driverModePath, newDriverModeSettings, true); + FileUtils.setFilePerms(driverModePath); + } catch (IOException e) { + System.err.println("Failed to create camera drivermode file: " + driverModePath.toString()); + } + } + } + + private void checkCalibration() { + if (!calibrationExists()) { + try { + List calibrations = new ArrayList<>(); + JacksonUtils.serializer(calibrationPath, calibrations.toArray(), true); + } catch (IOException e) { + System.err.println("Failed to create camera calibration file: " + calibrationPath.toString()); + } + } + } + + private boolean configFolderExists() { + return Files.exists(configFolderPath); + } + + private boolean configExists() { + return configFolderExists() && Files.exists(configPath); + } + + private boolean driverModeExists() { + return configFolderExists() && Files.exists(driverModePath); + } + + private boolean calibrationExists() { + return configFolderExists() && Files.exists(calibrationPath); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java new file mode 100644 index 000000000..9ca60c571 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java @@ -0,0 +1,53 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision._2.vision.VisionProcess; +import com.chameleonvision._2.vision.camera.USBCaptureProperties; +import com.chameleonvision._2.vision.enums.StreamDivisor; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; + +public class CameraJsonConfig { + public final double fov; + public final String path; + public final String name; + public final String nickname; + public final double tilt; + public final int videomode; + public final StreamDivisor streamDivisor; + + @JsonCreator + public CameraJsonConfig( + @JsonProperty("fov") double fov, + @JsonProperty("path") String path, + @JsonProperty("name") String name, + @JsonProperty("nickname") String nickname, + @JsonProperty("videomode") int videomode, + @JsonProperty("streamDivisor") StreamDivisor streamDivisor, + @JsonProperty("tilt") double tilt) { + this.fov = fov; + this.path = path; + this.name = name; + this.nickname = nickname; + this.videomode = videomode; + this.streamDivisor = streamDivisor; + this.tilt = tilt; + } + + public CameraJsonConfig(String path, String name) { + this.fov = USBCaptureProperties.DEFAULT_FOV; + this.path = path; + this.name = name; + this.nickname = name; + this.videomode = 0; + this.streamDivisor = StreamDivisor.NONE; + this.tilt = 0; + } + + public static CameraJsonConfig fromVisionProcess(VisionProcess process) { + USBCaptureProperties camProps = process.getCamera().getProperties(); + int videomode = camProps.getCurrentVideoModeIndex(); + StreamDivisor streamDivisor = process.cameraStreamer.getDivisor(); + double tilt = process.getCamera().getProperties().getTilt().getDegrees(); + return new CameraJsonConfig(camProps.getFOV(), camProps.path, camProps.name, camProps.getNickname(), videomode, streamDivisor, tilt); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java new file mode 100644 index 000000000..414afcefd --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java @@ -0,0 +1,126 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision._2.util.ProgramDirectoryUtilities; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision.common.util.Platform; +import com.chameleonvision.common.util.ShellExec; +import com.chameleonvision.common.util.file.FileUtils; +import com.chameleonvision.common.util.file.JacksonUtils; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.LinkedHashMap; +import java.util.List; + +public class ConfigManager { + private ConfigManager() { + } + + public static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings"); + private static final Path settingsFilePath = Paths.get(SettingsPath.toString(), "settings.json"); + + private static final LinkedHashMap cameraConfigs = new LinkedHashMap<>(); + + public static GeneralSettings settings = new GeneralSettings(); + + private static boolean settingsFolderExists() { + return Files.exists(SettingsPath); + } + + private static boolean settingsFileExists() { + return settingsFolderExists() && Files.exists(settingsFilePath); + } + + private static void checkSettingsFolder() { + if (!settingsFolderExists()) { + try { + if (!(new File(SettingsPath.toUri()).mkdirs())) { + System.err.println("Failed to create settings folder: " + SettingsPath.toString()); + } + Files.createDirectory(SettingsPath); + if (!Platform.CurrentPlatform.isWindows()) { + new ShellExec().executeBashCommand("sudo chmod -R 0777 " + SettingsPath.toString()); + } + } catch (IOException e) { + if (!(e instanceof java.nio.file.FileAlreadyExistsException)) + e.printStackTrace(); + } + } + } + + private static void checkSettingsFile() { + boolean settingsFileEmpty = settingsFileExists() && new File(settingsFilePath.toString()).length() == 0; + if (settingsFileEmpty || !settingsFileExists()) { + try { + JacksonUtils.serializer(settingsFilePath, settings, true); + FileUtils.setFilePerms(settingsFilePath); + } catch (IOException e) { + e.printStackTrace(); + } + } else { + try { + settings = JacksonUtils.deserialize(settingsFilePath, GeneralSettings.class); + } catch (IOException e) { + System.err.println("Failed to load settings.json, using defaults."); + } + } + } + + public static void initializeSettings() { + System.out.println("Settings folder: " + SettingsPath.toString()); + checkSettingsFolder(); + checkSettingsFile(); + FileUtils.setAllPerms(SettingsPath); + } + + private static void saveSettingsFile() { + try { + JacksonUtils.serializer(settingsFilePath, settings, true); + FileUtils.setFilePerms(settingsFilePath); + } catch (IOException e) { + System.err.println("Failed to save settings.json!"); + } + } + + public static void saveGeneralSettings() { + checkSettingsFolder(); + saveSettingsFile(); + } + + public static List initializeCameras(List preliminaryConfigs) { + List configList = new ArrayList<>(); + + checkSettingsFolder(); + + // loop over all the camera names and try to create settings folders for it + for (CameraJsonConfig preliminaryConfig : preliminaryConfigs) { + CameraConfig cameraConfiguration = new CameraConfig(preliminaryConfig); + cameraConfigs.put(preliminaryConfig.name, cameraConfiguration); + + FullCameraConfiguration camJsonConfig = cameraConfiguration.load(); + + configList.add(camJsonConfig); + } + + return configList; + } + + public static void saveCameraConfig(String cameraName, CameraJsonConfig config) { + var camConf = cameraConfigs.get(cameraName); + camConf.saveConfig(config); + } + + public static void saveCameraPipelines(String cameraName, List pipelines) { + var camConf = cameraConfigs.get(cameraName); + camConf.savePipelines(pipelines); + } + + public static void saveCameraDriverMode(String cameraName, CVPipelineSettings driverMode) { + var camConf = cameraConfigs.get(cameraName); + camConf.saveDriverMode(driverMode); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java new file mode 100644 index 000000000..b4c66e083 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java @@ -0,0 +1,21 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; + +import java.util.List; + +public class FullCameraConfiguration { + public final CameraJsonConfig cameraConfig; + public final List pipelines; + public final CVPipelineSettings driverMode; + public final List calibration; + public final CameraConfig fileConfig; + + FullCameraConfiguration(CameraJsonConfig cameraConfig, List pipelines, CVPipelineSettings driverMode, List calibration, CameraConfig fileConfig) { + this.cameraConfig = cameraConfig; + this.pipelines = pipelines; + this.driverMode = driverMode; + this.calibration = calibration; + this.fileConfig = fileConfig; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java new file mode 100644 index 000000000..647b65f71 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java @@ -0,0 +1,14 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision.common.networking.NetworkMode; + +public class GeneralSettings { + public int teamNumber = 1577; + public NetworkMode connectionType = NetworkMode.DHCP; + public String ip = ""; + public String gateway = ""; + public String netmask = ""; + public String hostname = "Chameleon-vision"; + public String currentCamera = ""; + public Integer currentPipeline = null; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java new file mode 100644 index 000000000..69b8341ce --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java @@ -0,0 +1,74 @@ +package com.chameleonvision._2.config; + +import com.fasterxml.jackson.annotation.JsonProperty; +import org.opencv.core.CvType; +import org.opencv.core.Mat; + +import java.util.Arrays; + + +public class JsonMat { + public final int rows; + public final int cols; + public final int type; + public final double[] data; + + public JsonMat(int rows, int cols, double[] data) { + this(rows, cols, CvType.CV_64FC1, data); + } + + public JsonMat( + @JsonProperty("rows") int rows, + @JsonProperty("cols") int cols, + @JsonProperty("type") int type, + @JsonProperty("data") double[] data) { + this.rows = rows; + this.cols = cols; + this.type = type; + this.data = data; + } + + public Mat toMat() { + return toMat(this); + } + + private static boolean isCameraMatrixMat(Mat mat) { + return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3; + } + + private static boolean isDistortionCoeffsMat(Mat mat) { + return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1; + } + + private static boolean isCalibrationMat(Mat mat) { + return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat); + } + + public static double[] getDataFromMat(Mat mat) { + if (!isCalibrationMat(mat)) return null; + + double[] data = new double[(int)(mat.total()*mat.elemSize())]; + mat.get(0, 0, data); + + int dataLen = -1; + + if (isCameraMatrixMat(mat)) dataLen = 9; + if (isDistortionCoeffsMat(mat)) dataLen = 5; + + // truncate Mat data to correct number data points. + return Arrays.copyOfRange(data, 0, dataLen); + } + + public static JsonMat fromMat(Mat mat) { + if (!isCalibrationMat(mat)) return null; + return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat)); + } + + public static Mat toMat(JsonMat jsonMat) { + if (jsonMat.type != CvType.CV_64FC1) return null; + + Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type); + retMat.put(0, 0, jsonMat.data); + return retMat; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java new file mode 100644 index 000000000..fdc95042b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java @@ -0,0 +1,144 @@ +package com.chameleonvision._2.config; + +import com.chameleonvision._2.config.serializers.StandardCVPipelineSettingsDeserializer; +import com.chameleonvision._2.config.serializers.StandardCVPipelineSettingsSerializer; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.chameleonvision.common.util.file.FileUtils; +import com.chameleonvision.common.util.file.JacksonUtils; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; + +public class PipelineConfig { + + private final CameraConfig cameraConfig; + + /** + * Construct a new PipelineConfig + * + * @param cameraConfig the CameraConfig (parent folder, kinda?) + */ + PipelineConfig(CameraConfig cameraConfig) { + this.cameraConfig = cameraConfig; + } + + private void checkFolder() { + if (!(new File(cameraConfig.pipelineFolderPath.toUri()).mkdirs())) { + if (Files.notExists(cameraConfig.pipelineFolderPath)) { + System.err.println("Failed to create pipelines folder."); + } + } + try { + FileUtils.setFilePerms(cameraConfig.pipelineFolderPath); + } catch (IOException e) { + // ignored + } + } + + private File[] getPipelineFiles() { + return new File(cameraConfig.pipelineFolderPath.toUri()).listFiles(); + } + + private boolean folderHasPipelines() { + File[] folderContents = getPipelineFiles(); + if (folderContents == null) return false; + return folderContents.length > 0; + } + + void check() { + cameraConfig.checkFolder(); + checkFolder(); + // Check if there's at least one pipe + if (!folderHasPipelines()) { + save(new StandardCVPipelineSettings()); + } + } + + private Path getPipelinePath(CVPipelineSettings setting) { + String pipelineName = setting.nickname.replace(' ', '_'); + String fullFileName = pipelineName + ".json"; + return Path.of(cameraConfig.pipelineFolderPath.toString(), fullFileName); + } + + private boolean pipelineExists(CVPipelineSettings setting) { + return Files.exists(getPipelinePath(setting)); + } + + public void save(CVPipelineSettings settings) { + + var path = getPipelinePath(settings); + + if (settings instanceof StandardCVPipelineSettings) { + try { + JacksonUtils.serialize(path, (StandardCVPipelineSettings) settings, StandardCVPipelineSettings.class, new StandardCVPipelineSettingsSerializer(), true); + FileUtils.setFilePerms(path); + } catch (IOException e) { + e.printStackTrace(); + } + } else { + try { + JacksonUtils.serializer(path, settings, true); + FileUtils.setFilePerms(path); + } catch (IOException e) { + e.printStackTrace(); + } + } + } + + public void save(List settings) { + for (CVPipelineSettings setting : settings) { + save(setting); + } + } + + public void delete(CVPipelineSettings setting) { + if (pipelineExists(setting)) { + try { + Files.delete(getPipelinePath(setting)); + } catch (IOException e) { + System.err.println("Failed to delete pipeline!"); + } + } + } + + public CVPipelineSettings rename(CVPipelineSettings setting, String newName) { + if (pipelineExists(setting)) { + delete(setting); + setting.nickname = newName; + save(setting); + } else { + setting.nickname = newName; + save(setting); + } + return setting; + } + + public List load() { + check(); // TODO: this ensures there will be a default pipeline. is the check later necessary? + + File[] pipelineFiles = getPipelineFiles(); + List deserializedList = new ArrayList<>(); + + if (pipelineFiles == null || pipelineFiles.length < 1) { + // TODO handle no pipelines to load + System.err.println("no pipes to load! loading default"); + } else { + for (File pipelineFile : pipelineFiles) { + try { + var pipe = JacksonUtils.deserialize(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class, new StandardCVPipelineSettingsDeserializer()); + deserializedList.add(pipe); + } catch (IOException e) { + System.err.println("couldn't load cvpipeline2d"); + } + } + } + + return deserializedList; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java new file mode 100644 index 000000000..863e4ba11 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java @@ -0,0 +1,130 @@ +package com.chameleonvision._2.config.serializers; + +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.util.numbers.IntegerCouple; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.deser.std.StdDeserializer; +import com.fasterxml.jackson.databind.type.CollectionType; +import com.fasterxml.jackson.databind.type.TypeFactory; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +public abstract class BaseDeserializer extends StdDeserializer { + protected BaseDeserializer(Class vc) { + super(vc); + } + + JsonNode baseNode; + + private static final CollectionType numberListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Number.class); + private CollectionType pointListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Object.class); + private static final ObjectMapper mapper = new ObjectMapper(); + private static boolean nodeGood(JsonNode node) { + return node != null && !node.toString().equals(""); + } + + IntegerCouple getNumberCouple(String name, IntegerCouple defaultValue) throws JsonProcessingException { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + List mapped = mapper.readValue(node.toString(), numberListColType); + return new IntegerCouple(mapped.get(0), mapped.get(1)); + } + return defaultValue; + } + + DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue) throws JsonProcessingException { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + List mapped = mapper.readValue(node.toString(), numberListColType); + return new DoubleCouple(mapped.get(0), mapped.get(1)); + } + return defaultValue; + } + + List getNumberList(String name, List defaultValue) throws JsonProcessingException { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + return mapper.readValue(node.toString(), numberListColType); + } + return defaultValue; + } + + boolean getBoolean(String name, boolean defaultValue) { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + return node.booleanValue(); + } + + return defaultValue; + } + + int getInt(String name, int defaultValue) { + return (int) getDouble(name, defaultValue); + } + + double getDouble(String name, double defaultValue) { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + return node.numberValue().doubleValue(); + } + + return defaultValue; + } + + String getString(String name, String defaultValue) { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + return node.asText(); + } + + return defaultValue; + } + + > E getEnum(String name, Class enumClass, E defaultValue) throws IOException { + JsonNode node = baseNode.get(name); + + if (nodeGood(node)) { + E[] possibleVals = enumClass.getEnumConstants(); + String jsonVal = baseNode.get(name).asText(); + + for (E val : possibleVals) { + if (val.name().equals(jsonVal)) { + return val; + } + } + } + + return defaultValue; + } + MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) throws JsonProcessingException { + JsonNode node = baseNode.get(name); + if (nodeGood(node)){ + List> numberList = mapper.readValue(node.toString(), pointListColType); + List point3List = new ArrayList<>(); + for (List tmp : numberList){ + Point3 p = new Point3(); + p.x = tmp.get(0).doubleValue(); + p.y = tmp.get(1).doubleValue(); + p.z = tmp.get(2).doubleValue(); + point3List.add(p); + } + MatOfPoint3f mat = new MatOfPoint3f(); + mat.fromList(point3List); + return mat; + } + + return defaultValue; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java new file mode 100644 index 000000000..6826332ca --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java @@ -0,0 +1,49 @@ +package com.chameleonvision._2.config.serializers; + +import com.chameleonvision.common.util.numbers.NumberCouple; +import com.fasterxml.jackson.core.JsonGenerator; +import com.fasterxml.jackson.databind.ser.std.StdSerializer; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; + +import java.io.IOException; +import java.util.List; + +public abstract class BaseSerializer extends StdSerializer { + protected BaseSerializer(Class t) { + super(t); + } + + JsonGenerator generator; + + void writeNumberCoupleAsNumberArray(String name, N couple) throws IOException { + generator.writeArrayFieldStart(name); + generator.writeObject(couple.getFirst()); + generator.writeObject(couple.getSecond()); + generator.writeEndArray(); + } + + void writeNumberListAsNumberArray(String name, List list) throws IOException { + generator.writeArrayFieldStart(name); + for (Number i : list) { + generator.writeObject(i); + } + generator.writeEndArray(); + } + + > void writeEnum(String name, E num) throws IOException { + generator.writeFieldName(name); + generator.writeString(num.name()); + } + + void writeMatOfPoint3f(String name, MatOfPoint3f mat) throws IOException { + List point3List = mat.toList(); + generator.writeArrayFieldStart(name); + + for (Point3 point3 : point3List) { + double[] tmp = {point3.x, point3.y, point3.z}; + generator.writeObject(tmp); + } + generator.writeEndArray(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java new file mode 100644 index 000000000..679545cba --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java @@ -0,0 +1,79 @@ +package com.chameleonvision._2.config.serializers; + +import com.chameleonvision._2.vision.enums.*; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.fasterxml.jackson.core.JsonParser; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationContext; + +import java.io.IOException; + +public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer { + public StandardCVPipelineSettingsDeserializer() { + this(null); + } + + private StandardCVPipelineSettingsDeserializer(Class vc) { + super(vc); + } + + @Override + public StandardCVPipelineSettings deserialize(JsonParser jsonParser, DeserializationContext ctxt) throws IOException, JsonProcessingException { + // set BaseDeserializer parser reference. + baseNode = jsonParser.getCodec().readTree(jsonParser); + + StandardCVPipelineSettings pipeline = new StandardCVPipelineSettings(); + + pipeline.index = getInt("index", pipeline.index); + + pipeline.flipMode = getEnum("flipMode", ImageFlipMode.class, pipeline.flipMode); + pipeline.rotationMode = getEnum("rotationMode", ImageRotationMode.class, pipeline.rotationMode); + + pipeline.nickname = getString("nickname", pipeline.nickname); + + pipeline.exposure = getDouble("exposure", pipeline.exposure); + pipeline.brightness = getDouble("brightness", pipeline.brightness); + pipeline.gain = getDouble("gain", pipeline.gain); + + pipeline.videoModeIndex = getInt("videoModeIndex", pipeline.videoModeIndex); + + pipeline.streamDivisor = getEnum("streamDivisor", StreamDivisor.class, pipeline.streamDivisor); + + pipeline.hue = getNumberCouple("hue", pipeline.hue); + pipeline.saturation = getNumberCouple("saturation", pipeline.saturation); + pipeline.value = getNumberCouple("value", pipeline.value); + + pipeline.erode = getBoolean("erode", pipeline.erode); + pipeline.dilate = getBoolean("dilate", pipeline.dilate); + + pipeline.area = getNumberCouple("area", pipeline.area); + pipeline.ratio = getNumberCouple("ratio", pipeline.ratio); + pipeline.extent = getNumberCouple("extent", pipeline.extent); + + pipeline.speckle = getInt("speckle", (Integer) pipeline.speckle); + + pipeline.isBinary = getBoolean("isBinary", pipeline.isBinary); + + pipeline.sortMode = getEnum("sortMode", SortMode.class, pipeline.sortMode); + pipeline.targetRegion = getEnum("targetRegion", TargetRegion.class, pipeline.targetRegion); + pipeline.targetOrientation = getEnum("targetOrientation", TargetOrientation.class, pipeline.targetOrientation); + + pipeline.multiple = getBoolean("multiple", pipeline.multiple); + + pipeline.targetGroup = getEnum("targetGroup", TargetGroup.class, pipeline.targetGroup); + pipeline.targetIntersection = getEnum("targetIntersection", TargetIntersection.class, pipeline.targetIntersection); + + pipeline.point = getNumberCouple("point", pipeline.point); + + pipeline.calibrationMode = getEnum("calibrationMode", CalibrationMode.class, pipeline.calibrationMode); + + pipeline.dualTargetCalibrationM = getDouble("dualTargetCalibrationM", pipeline.dualTargetCalibrationM); + pipeline.dualTargetCalibrationB = getDouble("dualTargetCalibrationB", pipeline.dualTargetCalibrationB); + + pipeline.is3D = getBoolean("is3D", pipeline.is3D); + pipeline.targetCornerMat = getMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat); + pipeline.accuracy = getDouble("accuracy", pipeline.accuracy.doubleValue()); + + return pipeline; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsSerializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsSerializer.java new file mode 100644 index 000000000..c1255871a --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsSerializer.java @@ -0,0 +1,82 @@ +package com.chameleonvision._2.config.serializers; + +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.fasterxml.jackson.core.JsonGenerator; +import com.fasterxml.jackson.databind.SerializerProvider; + +import java.io.IOException; + +public class StandardCVPipelineSettingsSerializer extends BaseSerializer { + public StandardCVPipelineSettingsSerializer() { + this(null); + } + + private StandardCVPipelineSettingsSerializer(Class t) { + super(t); + } + + @Override + public void serialize(StandardCVPipelineSettings pipeline, JsonGenerator gen, SerializerProvider provider) throws IOException { + // set BaseSerializer generator reference. + generator = gen; + + gen.writeStartObject(); + + gen.writeNumberField("index", pipeline.index); + + writeEnum("flipMode", pipeline.flipMode); + writeEnum("rotationMode", pipeline.rotationMode); + + gen.writeStringField("nickname", pipeline.nickname); + + gen.writeNumberField("exposure", pipeline.exposure); + gen.writeNumberField("brightness", pipeline.brightness); + gen.writeNumberField("gain", pipeline.gain); + + gen.writeNumberField("videoModeIndex", pipeline.videoModeIndex); + + writeEnum("streamDivisor", pipeline.streamDivisor); + + writeNumberCoupleAsNumberArray("hue", pipeline.hue); + writeNumberCoupleAsNumberArray("saturation", pipeline.saturation); + writeNumberCoupleAsNumberArray("value", pipeline.value); + + gen.writeBooleanField("erode", pipeline.erode); + gen.writeBooleanField("dilate", pipeline.dilate); + + writeNumberCoupleAsNumberArray("area", pipeline.area); + writeNumberCoupleAsNumberArray("ratio", pipeline.ratio); + writeNumberCoupleAsNumberArray("extent", pipeline.extent); + + // speckle rejection + gen.writeNumberField("speckle", (Integer) pipeline.speckle); + + // stream output (camera feed, or thresholded feed) + gen.writeBooleanField("isBinary", pipeline.isBinary); + + writeEnum("sortMode", pipeline.sortMode); + writeEnum("targetRegion", pipeline.targetRegion); + writeEnum("targetOrientation", pipeline.targetOrientation); + + // show multiple targets when drawing + gen.writeBooleanField("multiple", pipeline.multiple); + + writeEnum("targetGroup", pipeline.targetGroup); + writeEnum("targetIntersection", pipeline.targetIntersection); + + // single calibration point + writeNumberCoupleAsNumberArray("point", pipeline.point); + + // target X/Y calibration + writeEnum("calibrationMode", pipeline.calibrationMode); + + // TODO: better names? or use an array? + gen.writeNumberField("dualTargetCalibrationM", pipeline.dualTargetCalibrationM); + gen.writeNumberField("dualTargetCalibrationB", pipeline.dualTargetCalibrationB); + + gen.writeBooleanField("is3D", pipeline.is3D); + writeMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat); + gen.writeNumberField("accuracy", pipeline.accuracy.doubleValue()); + gen.writeEndObject(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/LinuxNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/LinuxNetworking.java new file mode 100644 index 000000000..76305d044 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/LinuxNetworking.java @@ -0,0 +1,103 @@ +package com.chameleonvision._2.network; + +import org.apache.commons.io.FileUtils; + +import java.io.File; +import java.io.IOException; +import java.net.InetAddress; +import java.net.SocketException; +import java.nio.charset.StandardCharsets; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class LinuxNetworking extends SysNetworking { + private static final String PATH = "/etc/dhcpcd.conf"; + + @Override + public boolean setDHCP() { + File dhcpConf = new File(PATH); + if (dhcpConf.exists()) { + try { + List lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8); + for (int i = 0; i < lines.size(); i++) { + String line = lines.get(i); + if (line.startsWith("interface " + networkInterface.name)) { + lines.remove(i); + for (int j = i; j < lines.size(); j++) { + String subInterface = lines.get(j); + if (subInterface.contains("static ip_address") || subInterface.contains("static routers")) { + lines.remove(j); + j--; + } + if (subInterface.contains("interface")) { + break; + } + } + FileUtils.writeLines(dhcpConf, lines); + return true; + } + } + } catch (IOException e) { + e.printStackTrace(); + return false; + } + + } else { + System.err.println("dhcpcd5 is not installed cant set ip"); + return false; + } + return true; + } + + @Override + public boolean setHostname(String newHostname) { + String[] setHostnameArgs = {"set-hostname", newHostname}; + try { + var setHostnameRetCode = shell.execute("hostnamectl", setHostnameArgs); + return setHostnameRetCode == 0; + } catch (Exception e) { + e.printStackTrace(); + return false; + } + } + + @Override + public boolean setStatic(String ipAddress, String netmask, String gateway) { + setDHCP(); // clean up old static interface + File dhcpConf = new File(PATH); + try { + List lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8); + lines.add("interface " + networkInterface.name); + InetAddress iNetMask = InetAddress.getByName(netmask); + int prefix = NetmaskToCIDR.convertNetmaskToCIDR(iNetMask); + lines.add("static ip_address=" + ipAddress + "/" + prefix); + lines.add("static routers=" + gateway); + FileUtils.writeLines(dhcpConf, lines); + return true; + } catch (IOException e) { + e.printStackTrace(); + } + return false; + } + + + @Override + public List getNetworkInterfaces() throws SocketException { + List netInterfaces; + try { + netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces()); + } catch (SocketException e) { + return null; + } + List goodInterfaces = new ArrayList<>(); + + for (var netInterface : netInterfaces) { + if (netInterface.getDisplayName().contains("lo")) continue; + if (!netInterface.isUp()) continue; + goodInterfaces.add(netInterface); + } + return goodInterfaces; + + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java new file mode 100644 index 000000000..d68a2074a --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java @@ -0,0 +1,29 @@ +package com.chameleonvision._2.network; + +import java.net.InetAddress; + +public class NetmaskToCIDR { + //code belongs to https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java + public static int convertNetmaskToCIDR(InetAddress netmask) { + + byte[] netmaskBytes = netmask.getAddress(); + int cidr = 0; + boolean zero = false; + for (byte b : netmaskBytes) { + int mask = 0x80; + + for (int i = 0; i < 8; i++) { + int result = b & mask; + if (result == 0) { + zero = true; + } else if (zero) { + throw new IllegalArgumentException("Invalid netmask."); + } else { + cidr++; + } + mask >>>= 1; + } + } + return cidr; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java new file mode 100644 index 000000000..6b369e692 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java @@ -0,0 +1,54 @@ +package com.chameleonvision._2.network; + +import java.net.InterfaceAddress; + +@SuppressWarnings("WeakerAccess") +public class NetworkInterface { + public final String name; + public final String displayName; + public final String IPAddress; + public final String Netmask; + public final String Gateway; + public final String Broadcast; + + public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) { + name = inetface.getName(); + displayName = inetface.getDisplayName(); + + var inetAddress = ifaceAddress.getAddress(); + IPAddress = inetAddress.getHostAddress(); + Netmask = getIPv4LocalNetMask(ifaceAddress); + + // TODO: (low) hack to "get" gateway, this is gross and bad, pls fix + var splitIPAddr = IPAddress.split("\\."); + splitIPAddr[3] = "1"; + Gateway = String.join(".", splitIPAddr); + splitIPAddr[3] = "255"; + Broadcast = String.join(".", splitIPAddr); + } + + private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) { + var netPrefix = interfaceAddress.getNetworkPrefixLength(); + try { + // Since this is for IPv4, it's 32 bits, so set the sign value of + // the int to "negative"... + int shiftby = (1<<31); + // For the number of bits of the prefix -1 (we already set the sign bit) + for (int i = netPrefix - 1; i > 0; i--) { + // Shift the sign right... Java makes the sign bit sticky on a shift... + // So no need to "set it back up"... + shiftby = (shiftby >> 1); + } + // Transform the resulting value in xxx.xxx.xxx.xxx format, like if + /// it was a standard address... + // Return the address thus created... + return ((shiftby >> 24) & 255) + "." + ((shiftby >> 16) & 255) + "." + ((shiftby >> 8) & 255) + "." + (shiftby & 255); +// return InetAddress.getByName(maskString); + } + catch(Exception e) { + e.printStackTrace(); + } + // Something went wrong here... + return null; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java new file mode 100644 index 000000000..be5e28ea7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java @@ -0,0 +1,106 @@ +package com.chameleonvision._2.network; + +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision.common.util.Platform; + +import java.net.SocketException; +import java.util.ArrayList; +import java.util.List; + +public class NetworkManager { + private NetworkManager() { + } + + private static SysNetworking networking; + private static boolean isManaged = false; + + public static void initialize(boolean manage) { + isManaged = manage; + if (!isManaged) { + return; + } + + Platform platform = Platform.CurrentPlatform; + + if (platform.isLinux()) { + networking = new LinuxNetworking(); + } else if (platform.isWindows()) { +// networking = new WindowsNetworking(); + System.out.println("Windows networking is not yet supported. Running unmanaged."); + return; + } + + if (networking == null) { + throw new RuntimeException("Failed to detect platform!"); + } + + List interfaces = new ArrayList<>(); + List goodInterfaces = new ArrayList<>(); + + try { + interfaces = networking.getNetworkInterfaces(); + } catch (SocketException e) { + e.printStackTrace(); + } + + var teamBytes = NetworkManager.GetTeamNumberIPBytes(ConfigManager.settings.teamNumber); + + if (interfaces.size() > 0) { + for (var inetface : interfaces) { + for (var inetfaceAddr : inetface.getInterfaceAddresses()) { + var rawAddr = inetfaceAddr.getAddress().getAddress(); + if (rawAddr.length > 4) continue; + if (rawAddr[1] == teamBytes[0] && rawAddr[2] == teamBytes[1]) { + goodInterfaces.add(new NetworkInterface(inetface, inetfaceAddr)); + } + } + } + + if (goodInterfaces.size() == 0) { + isManaged = false; + System.err.println("No valid network interfaces found! Staying unmanaged."); + return; + } + + NetworkInterface botInterface = goodInterfaces.get(0); + networking.setNetworkInterface(botInterface); + } else { + isManaged = false; + System.err.println("No valid network interfaces found! Staying unmanaged."); + } + } + + private static byte[] GetTeamNumberIPBytes(int teamNumber) { + return new byte[]{(byte) (teamNumber / 100), (byte) (teamNumber % 100)}; + } + + + private static boolean setDHCP() { + if (!isManaged) { + return true; + } + return networking.setDHCP(); + } + + private static boolean setStatic(String ipAddress, String netmask, String gateway) { + if (!isManaged) { + return true; + } + return networking.setStatic(ipAddress, netmask, gateway); + } + + public static boolean setHostname(String hostname) { + if (!isManaged) { + return true; + } + return networking.setHostname(hostname); + } + + public static boolean setNetwork(boolean isStatic, String ip, String netmask, String gateway) { + if (isStatic) { + return setStatic(ip, netmask, gateway); + } else { + return setDHCP(); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java new file mode 100644 index 000000000..707a9c384 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java @@ -0,0 +1,36 @@ +package com.chameleonvision._2.network; + +import com.chameleonvision.common.util.ShellExec; + +import java.io.IOException; +import java.net.SocketException; +import java.util.List; + +public abstract class SysNetworking { + + NetworkInterface networkInterface; + ShellExec shell = new ShellExec(true, true); + + public String getHostname() { + try { + var retCode = shell.execute("hostname", null, true); + if (retCode == 0) { + while(!shell.isOutputCompleted()) {} + return shell.getOutput(); + } else { + return null; + } + } catch (IOException e) { + return null; + } + } + + public void setNetworkInterface(NetworkInterface networkInterface) { + this.networkInterface = networkInterface; + } + public abstract boolean setDHCP(); + public abstract boolean setHostname(String hostname); + public abstract boolean setStatic(String ipAddress, String netmask, String gateway); + public abstract List getNetworkInterfaces() throws SocketException; + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java new file mode 100644 index 000000000..9e77d3b8b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java @@ -0,0 +1,54 @@ +package com.chameleonvision._2.network; + +import java.net.SocketException; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class WindowsNetworking extends SysNetworking { + + @Override + public boolean setDHCP() { + return false; + } + + @Override + public boolean setHostname(String newHostname) { + var currentHostname = getHostname(); + + if (getHostname() == null) { + return false; + } + + String command = String.format("wmic computersystem where name=\"%s\" call rename name=\"%s\"", currentHostname, newHostname); + + try { + var process = Runtime.getRuntime().exec(command); + var returnCode = process.waitFor(); + return returnCode == 0; + } catch(Exception e) { + return false; + } + } + + @Override + public boolean setStatic(String ipAddress, String netmask, String gateway) { + return false; + } + + @Override + public List getNetworkInterfaces() throws SocketException { + var netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces()); + + List goodInterfaces = new ArrayList<>(); + + for (var netInterface : netInterfaces) { + if (netInterface.getDisplayName().toLowerCase().contains("bluetooth")) continue; + if (netInterface.getDisplayName().toLowerCase().contains("virtual")) continue; + if (netInterface.getDisplayName().toLowerCase().contains("loopback")) continue; + if (!netInterface.isUp()) continue; + goodInterfaces.add(netInterface); + } + return goodInterfaces; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java b/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java new file mode 100644 index 000000000..2d48b5575 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java @@ -0,0 +1,54 @@ +package com.chameleonvision._2.util; + +import edu.wpi.cscore.VideoMode; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.io.Writer; +import java.nio.file.Path; +import java.util.HashMap; + +public class Helpers { + + // TODO: MOVE + public static HashMap VideoModeToHashMap(VideoMode videoMode) { + return new HashMap() {{ + put("width", videoMode.width); + put("height", videoMode.height); + put("fps", videoMode.fps); + put("pixelFormat", videoMode.pixelFormat.toString()); + }}; + } + + + // TODO: MOVE + private static final String kServicePath = "/etc/systemd/system/chameleonVision.service"; + private static final String kServiceString = "[Unit]\n" + + "Description=chameleon vision\n" + + "\n" + + "[Service]\n" + + "ExecStart=/usr/bin/java -jar %s \n" + + "StandardOutput=file:/var/log/chameleon.out.txt\n" + + "StandardError=file:/var/log/chameleon.err.txt\n" + + "Type=simple\n" + + "WorkingDirectory=/usr/local/bin\n" + + "\n" + + "[Install]\n" + + "WantedBy=multi-user.target\n" + + "\n"; + + + public static void setService(Path filePath) throws IOException, InterruptedException { + String newService = String.format(kServiceString, filePath.toString()); + File file = new File(kServicePath); + if (file.exists()) { + file.delete(); + } + Writer writer = new FileWriter(file, false); + writer.write(newService); + writer.close(); + Process p = Runtime.getRuntime().exec("systemctl enable chameleonVision.service"); + p.waitFor(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java b/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java new file mode 100644 index 000000000..90bb10545 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java @@ -0,0 +1,37 @@ +package com.chameleonvision._2.util; + +import java.io.File; +import java.net.URISyntaxException; + +public class ProgramDirectoryUtilities { + private static String getJarName() { + return new File(ProgramDirectoryUtilities.class.getProtectionDomain() + .getCodeSource() + .getLocation() + .getPath()) + .getName(); + } + + private static boolean runningFromJAR() { + String jarName = getJarName(); + return jarName.contains(".jar"); + } + + public static String getProgramDirectory() { + if (runningFromJAR()) { + return getCurrentJARDirectory(); + } else { + return System.getProperty("user.dir"); + } + } + + private static String getCurrentJARDirectory() { + try { + return new File(ProgramDirectoryUtilities.class.getProtectionDomain().getCodeSource().getLocation().toURI().getPath()).getParent(); + } catch (URISyntaxException exception) { + exception.printStackTrace(); + } + + return null; + } +} \ No newline at end of file diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java new file mode 100644 index 000000000..4c11e6dc3 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java @@ -0,0 +1,187 @@ +package com.chameleonvision._2.vision; + +import com.chameleonvision._2.config.*; +import com.chameleonvision._2.util.Helpers; +import com.chameleonvision._2.vision.camera.USBCameraCapture; +import com.chameleonvision._2.vision.pipeline.CVPipeline; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision.common.util.Platform; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.UsbCameraInfo; +import org.opencv.videoio.VideoCapture; + +import java.util.*; +import java.util.stream.Collectors; +import java.util.stream.Stream; + +@SuppressWarnings("rawtypes") +public class VisionManager { + private VisionManager() {} + + private static final LinkedHashMap usbCameraInfosByCameraName = new LinkedHashMap<>(); + private static final LinkedList loadedCameraConfigs = new LinkedList<>(); + private static final LinkedList visionProcesses = new LinkedList<>(); + + @SuppressWarnings("WeakerAccess") + private static class VisionProcessManageable { + public final int index; + public final String name; + public final VisionProcess visionProcess; + + public VisionProcessManageable(int index, String name, VisionProcess visionProcess) { + this.index = index; + this.name = name; + this.visionProcess = visionProcess; + } + } + + private static VisionProcess currentUIVisionProcess; + + public static boolean initializeSources() { + int suffix = 0; + for (UsbCameraInfo info : UsbCamera.enumerateUsbCameras()) { + VideoCapture cap = new VideoCapture(info.dev); + if (cap.isOpened()) { + cap.release(); + // Filter non-ascii characters because ext4 doesn't play nice with unicode in directory names + String name = info.name.replaceAll("[^\\x00-\\x7F]", ""); + while (usbCameraInfosByCameraName.containsKey(name)) { + suffix++; + name = String.format("%s (%d)", name, suffix); + } + usbCameraInfosByCameraName.put(name, info); + } + } + + if (usbCameraInfosByCameraName.isEmpty()) { + return false; + } + System.out.printf("[VisionManager] Found %s cameras!\n", usbCameraInfosByCameraName.size()); + + // load the config + List preliminaryConfigs = new ArrayList<>(); + + usbCameraInfosByCameraName.forEach((suffixedName, cameraInfo) -> { + String truePath; + + if (Platform.CurrentPlatform.isWindows()) { + truePath = cameraInfo.path; + } else { + truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path); + } + + preliminaryConfigs.add(new CameraJsonConfig(truePath, suffixedName)); + }); + + loadedCameraConfigs.addAll(ConfigManager.initializeCameras(preliminaryConfigs)); + System.out.printf("[VisionManager] Loaded %s cameras!\n", loadedCameraConfigs.size()); + return true; + } + + public static boolean initializeProcesses() { + for (int i = 0; i < loadedCameraConfigs.size(); i++) { + FullCameraConfiguration config = loadedCameraConfigs.get(i); + + CameraJsonConfig cameraJsonConfig = config.cameraConfig; + + USBCameraCapture camera = new USBCameraCapture(config); + VisionProcess process = new VisionProcess(camera, config); + process.pipelineManager.driverModePipeline.settings = config.driverMode; + visionProcesses.add(new VisionProcessManageable(i, cameraJsonConfig.name, process)); + } + currentUIVisionProcess = getVisionProcessByIndex(0); + ConfigManager.settings.currentCamera = visionProcesses.get(0).name; + + System.out.printf("[VisionManager] Loaded %s vision processes! Current process: %s\n", visionProcesses.size(), visionProcesses.get(0).name); + return true; + } + + public static void startProcesses() { + visionProcesses.forEach((vpm) -> vpm.visionProcess.start()); + } + + public static VisionProcess getCurrentUIVisionProcess() { + return currentUIVisionProcess; + } + + public static CameraConfig getCurrentCameraConfig() { + return getCameraConfig(currentUIVisionProcess); + } + + public static CameraConfig getCameraConfig(VisionProcess process) { + String cameraName = process.getCamera().getProperties().name; + return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig; + } + + public static void setCurrentProcessByIndex(int processIndex) { + if (processIndex > visionProcesses.size() - 1) { + return; + } + + currentUIVisionProcess = getVisionProcessByIndex(processIndex); + ConfigManager.settings.currentCamera = visionProcesses.get(processIndex).name; + } + + public static VisionProcess getVisionProcessByIndex(int processIndex) { + if (processIndex > visionProcesses.size() - 1) { + return null; + } + + VisionProcessManageable vpm = visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(null); + return vpm != null ? vpm.visionProcess : null; + } + + public static List getAllCameraNicknames() { + return visionProcesses.stream().map(vpm -> vpm.visionProcess.getCamera() + .getProperties().getNickname()).collect(Collectors.toList()); + } + + public static List getCurrentCameraPipelineNicknames() { + return currentUIVisionProcess.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings.nickname).collect(Collectors.toList()); + } + + + public static void saveAllCameras() { + visionProcesses.forEach((vpm) -> { + VisionProcess process = vpm.visionProcess; + String cameraName = process.getCamera().getProperties().name; + Stream pipelineStream = process.pipelineManager.pipelines.stream(); + List pipelines = process.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList()); + CVPipelineSettings driverMode = process.getDriverModeSettings(); + CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(process); + ConfigManager.saveCameraPipelines(cameraName, pipelines); + ConfigManager.saveCameraDriverMode(cameraName, driverMode); + ConfigManager.saveCameraConfig(cameraName, config); + }); + } + + private static String getCurrentCameraName() { + return currentUIVisionProcess.getCamera().getProperties().name; + } + + public static void saveCurrentCameraSettings() { + CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(currentUIVisionProcess); + ConfigManager.saveCameraConfig(getCurrentCameraName(), config); + } + + public static void saveCurrentCameraPipelines() { + currentUIVisionProcess.pipelineManager.saveAllPipelines(); + } + + public static void saveCurrentCameraDriverMode() { + currentUIVisionProcess.pipelineManager.saveDriverModeConfig(); + } + + private static List getCameraResolutionList(USBCameraCapture capture) { + return capture.getProperties().getVideoModes().stream().map(Helpers::VideoModeToHashMap).collect(Collectors.toList()); + } + + public static List getCurrentCameraResolutionList() { + return getCameraResolutionList(currentUIVisionProcess.getCamera()); + } + + public static int getCurrentUIVisionProcessIndex() { + VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null); + return vpm != null ? vpm.index : -1; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java new file mode 100644 index 000000000..26fcba551 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java @@ -0,0 +1,389 @@ +package com.chameleonvision._2.vision; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.config.CameraConfig; +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.config.FullCameraConfiguration; +import com.chameleonvision._2.vision.camera.CameraStreamer; +import com.chameleonvision._2.vision.camera.USBCameraCapture; +import com.chameleonvision._2.vision.pipeline.CVPipelineResult; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision._2.vision.pipeline.PipelineManager; +import com.chameleonvision._2.vision.pipeline.impl.DriverVisionPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.chameleonvision._2.web.SocketHandler; +import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager; +import com.chameleonvision.common.scripting.ScriptEventType; +import com.chameleonvision.common.scripting.ScriptManager; +import com.chameleonvision.common.util.math.MathUtils; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpiutil.CircularBuffer; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.stream.Collectors; + + +@SuppressWarnings("rawtypes") +public class VisionProcess { + + public final USBCameraCapture cameraCapture; + private final VisionProcessRunnable visionRunnable; + private final CameraConfig fileConfig; + public final CameraStreamer cameraStreamer; + public PipelineManager pipelineManager; + + private volatile CVPipelineResult lastPipelineResult; + + // network table stuff + private final NetworkTable defaultTable; + private NetworkTableInstance tableInstance; + private NetworkTableEntry ntPipelineEntry; + public NetworkTableEntry ntDriverModeEntry; + private int ntDriveModeListenerID; + private int ntPipelineListenerID; + private NetworkTableEntry ntYawEntry; + private NetworkTableEntry ntPitchEntry; + private NetworkTableEntry ntAuxListEntry; + private NetworkTableEntry ntAreaEntry; + private NetworkTableEntry ntLatencyEntry; + private NetworkTableEntry ntValidEntry; + private NetworkTableEntry ntPoseEntry; + private NetworkTableEntry ntFittedHeightEntry; + private NetworkTableEntry ntFittedWidthEntry; + private NetworkTableEntry ntBoundingHeightEntry; + private NetworkTableEntry ntBoundingWidthEntry; + private NetworkTableEntry ntTargetRotation; + + private ObjectMapper objectMapper = new ObjectMapper(); + + private long lastUIUpdateMs = 0; + + VisionProcess(USBCameraCapture cameraCapture, FullCameraConfiguration config) { + this.cameraCapture = cameraCapture; + + fileConfig = config.fileConfig; + + pipelineManager = new PipelineManager(this, config.pipelines); + + // Thread to put frames on the dashboard + this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor); + + // Thread to process vision data + this.visionRunnable = new VisionProcessRunnable(); + + // network table + defaultTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname()); + } + + public void start() { + System.out.printf("[%s Process] Creating network table...\n", getCamera().getProperties().getNickname()); + initNT(defaultTable); + + System.out.printf("[%s Process] Starting vision thread...\n", getCamera().getProperties().getNickname()); + var visionThread = new Thread(visionRunnable); + visionThread.setName(getCamera().getProperties().name + " - Vision Thread"); + visionThread.start(); + } + + /** + * Removes the old value change listeners + * calls {@link #initNT} + * + * @param newTable passed to {@link #initNT} + */ + public void resetNT(NetworkTable newTable) { + ntDriverModeEntry.removeListener(ntDriveModeListenerID); + ntPipelineEntry.removeListener(ntPipelineListenerID); + initNT(newTable); + } + + public void setCameraNickname(String newName) { + getCamera().getProperties().setNickname(newName); + NetworkTable camTable = NetworkTablesManager.kRootTable.getSubTable(newName); + resetNT(camTable); + } + + private void initNT(NetworkTable camTable) { + tableInstance = camTable.getInstance(); + ntPipelineEntry = camTable.getEntry("pipeline"); + ntDriverModeEntry = camTable.getEntry("driverMode"); + ntPitchEntry = camTable.getEntry("targetPitch"); + ntYawEntry = camTable.getEntry("targetYaw"); + ntAreaEntry = camTable.getEntry("targetArea"); + ntLatencyEntry = camTable.getEntry("latency"); + ntValidEntry = camTable.getEntry("isValid"); + ntAuxListEntry = camTable.getEntry("auxTargets"); + ntPoseEntry = camTable.getEntry("targetPose"); + ntFittedHeightEntry = camTable.getEntry("targetFittedHeight"); + ntFittedWidthEntry = camTable.getEntry("targetFittedWidth"); + ntBoundingHeightEntry = camTable.getEntry("targetBoundingHeight"); + ntBoundingWidthEntry = camTable.getEntry("targetBoundingWidth"); + ntTargetRotation = camTable.getEntry("targetRotation"); + ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate); + ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate); + ntDriverModeEntry.setBoolean(false); + ntPipelineEntry.setNumber(pipelineManager.getCurrentPipelineIndex()); + pipelineManager.ntIndexEntry = ntPipelineEntry; + } + + private void setDriverMode(EntryNotification driverModeEntryNotification) { + setDriverMode(driverModeEntryNotification.value.getBoolean()); + } + + public void setDriverMode(boolean driverMode) { + pipelineManager.setDriverMode(driverMode); + ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode); + SocketHandler.sendFullSettings(); + } + + /** + * Method called by the nt entry listener to update the next pipeline. + * + * @param notification the notification + */ + private void setPipeline(EntryNotification notification) { + var wantedPipelineIndex = (int) notification.value.getDouble(); + if (pipelineManager.pipelines.size() - 1 < wantedPipelineIndex) { + ntPipelineEntry.setDouble(pipelineManager.getCurrentPipelineIndex()); + } else { + pipelineManager.setCurrentPipeline(wantedPipelineIndex); + } + } + + public void setDriverModeEntry(boolean isDriverMode) { + // if it's null, we haven't even started the program yet, so just return + // otherwise, set it. + if (ntDriverModeEntry != null) { + ntDriverModeEntry.setBoolean(isDriverMode); + } + } + + private void updateUI(CVPipelineResult data) { + // 30 "FPS" update rate + long currentMillis = System.currentTimeMillis(); + if (currentMillis - lastUIUpdateMs > 1000 / 30) { + lastUIUpdateMs = currentMillis; + + + if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) { + HashMap WebSend = new HashMap<>(); + HashMap point = new HashMap<>(); + HashMap pointMap = new HashMap<>(); + ArrayList webTargets = new ArrayList<>(); + List center = new ArrayList<>(); + + + if (data.hasTarget) { + if (data instanceof StandardCVPipeline.StandardCVPipelineResult) { + StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data; + StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0); + try { + if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) { + for (var target : result.targets) { + pointMap = new HashMap<>(); + pointMap.put("pitch", target.pitch); + pointMap.put("yaw", target.yaw); + pointMap.put("area", target.area); + pointMap.put("pose", target.cameraRelativePose); + webTargets.add(pointMap); + } + } else { + pointMap.put("pitch", bestTarget.pitch); + pointMap.put("yaw", bestTarget.yaw); + pointMap.put("area", bestTarget.area); + pointMap.put("pose", bestTarget.cameraRelativePose); + webTargets.add(pointMap); + } + center.add(bestTarget.minAreaRect.center.x); + center.add(bestTarget.minAreaRect.center.y); + } catch (ClassCastException ignored) { + + } + } else { + pointMap.put("pitch", null); + pointMap.put("yaw", null); + pointMap.put("area", null); + pointMap.put("pose", new Pose2d()); + webTargets.add(pointMap); + center.add(null); + center.add(null); + } + + point.put("fps", visionRunnable.fps); + point.put("targets", webTargets); + point.put("rawPoint", center); + } else { + point.put("fps", visionRunnable.fps); + } + WebSend.put("point", point); + SocketHandler.broadcastMessage(WebSend); + } + } + } + + private void updateNetworkTableData(CVPipelineResult data) { + ntValidEntry.setBoolean(data.hasTarget); + if (data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) { + if (data instanceof StandardCVPipeline.StandardCVPipelineResult) { + + //noinspection unchecked + List targets = (List) data.targets; + StandardCVPipeline.TrackedTarget bestTarget = targets.get(0); + ntLatencyEntry.setDouble(MathUtils.roundTo(data.processTime * 1e-6, 3)); + ntPitchEntry.setDouble(bestTarget.pitch); + ntYawEntry.setDouble(bestTarget.yaw); + ntAreaEntry.setDouble(bestTarget.area); + ntBoundingHeightEntry.setDouble(bestTarget.boundingRect.height); + ntBoundingWidthEntry.setDouble(bestTarget.boundingRect.width); + ntFittedHeightEntry.setDouble(bestTarget.minAreaRect.size.height); + ntFittedWidthEntry.setDouble(bestTarget.minAreaRect.size.width); + ntTargetRotation.setDouble(bestTarget.minAreaRect.angle); + try { + Pose2d targetPose = targets.get(0).cameraRelativePose; + double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()}; + ntPoseEntry.setDoubleArray(targetArray); +// ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose)); + ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream() + .map(it -> List.of(it.pitch, it.yaw, it.area, it.boundingRect.width, it.boundingRect.height, it.minAreaRect.size.width, it.minAreaRect.size.height, it.minAreaRect.angle, it.cameraRelativePose)) + .collect(Collectors.toList()))); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } else { + ntPitchEntry.setDouble(0.0); + ntYawEntry.setDouble(0.0); + ntAreaEntry.setDouble(0.0); + ntLatencyEntry.setDouble(0.0); + ntAuxListEntry.setString(""); + } + } + tableInstance.flush(); + } + + public void setVideoMode(VideoMode newMode) { + cameraCapture.setVideoMode(newMode); + cameraStreamer.setNewVideoMode(newMode); + } + + public VideoMode getCurrentVideoMode() { + return cameraCapture.getCurrentVideoMode(); + } + + public List getPossibleVideoModes() { + return cameraCapture.getProperties().videoModes; + } + + public USBCameraCapture getCamera() { + return cameraCapture; + } + + public CVPipelineSettings getDriverModeSettings() { + return pipelineManager.driverModePipeline.settings; + } + + public void addCalibration(CameraCalibrationConfig cal) { + cameraCapture.addCalibrationData(cal); + System.out.println("saving to file"); + fileConfig.saveCalibration(cameraCapture.getAllCalibrationData()); + } + + public void setIs3d(Boolean value) { + var settings = pipelineManager.getCurrentPipeline().settings; + if (settings instanceof StandardCVPipelineSettings) { + ((StandardCVPipelineSettings) settings).is3D = value; + } + } + + public boolean getIs3d() { + var settings = pipelineManager.getCurrentPipeline().settings; + if (settings instanceof StandardCVPipelineSettings) { + return ((StandardCVPipelineSettings) settings).is3D; + } + return false; + } + + /** + * VisionProcessRunnable will process images as quickly as possible + */ + private class VisionProcessRunnable implements Runnable { + + volatile Double fps = 0.0; + private CircularBuffer fpsAveragingBuffer = new CircularBuffer(7); + + @Override + public void run() { + var lastUpdateTimeNanos = System.nanoTime(); + var lastStreamTimeMs = System.currentTimeMillis(); + + System.out.printf("[%s Process] Vision Process Thread -- first run!\n", getCamera().getProperties().getNickname()); + + while (!Thread.interrupted()) { + + // blocking call, will block until camera has a new frame. + Pair camData = cameraCapture.getFrame(); + + + Mat camFrame = camData.getLeft(); + if (camFrame.cols() > 0 && camFrame.rows() > 0) { + CVPipelineResult result = null; + try { + result = pipelineManager.getCurrentPipeline().runPipeline(camFrame); + } catch (Exception e) { + System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!"); + e.printStackTrace(); + } + + camFrame.release(); + + if (result != null) { + result.setTimestamp(camData.getRight()); + lastPipelineResult = result; + updateNetworkTableData(lastPipelineResult); + updateUI(lastPipelineResult); + } + } + + try { + var currentTime = System.currentTimeMillis(); + if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) { + if(lastPipelineResult != null) { + cameraStreamer.runStream(lastPipelineResult.outputMat); + lastStreamTimeMs = currentTime; + lastPipelineResult.outputMat.release(); + } else { + System.err.printf("[%s Process] Last pipeline result was null!\n", getCamera().getProperties().getNickname()); + } + } + + } catch (Exception e) { +// Debug.printInfo("Vision running faster than stream."); + System.err.printf("[%s Process] Exception in vision thread!\n", getCamera().getProperties().getNickname()); + e.printStackTrace(); + } + + var deltaTimeNanos = System.nanoTime() - lastUpdateTimeNanos; + fpsAveragingBuffer.addFirst(1.0 / (deltaTimeNanos * 1E-09)); + lastUpdateTimeNanos = System.nanoTime(); + fps = getAverageFPS(); + } + } + + double getAverageFPS() { + var temp = 0.0; + for (int i = 0; i < 7; i++) { + temp += fpsAveragingBuffer.get(i); + } + temp /= 7.0; + return temp; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java new file mode 100644 index 000000000..8d13b8a91 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java @@ -0,0 +1,48 @@ +package com.chameleonvision._2.vision.camera; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.vision.image.CaptureProperties; +import com.chameleonvision._2.vision.image.ImageCapture; +import edu.wpi.cscore.VideoMode; + +import java.util.List; + +public interface CameraCapture extends ImageCapture { + CaptureProperties getProperties(); + + VideoMode getCurrentVideoMode(); + + /** + * Set the exposure of the camera + * @param exposure the new exposure to set the camera to + */ + void setExposure(int exposure); + + /** + * Set the brightness of the camera + * @param brightness the new brightness to set the camera to + */ + void setBrightness(int brightness); + + /** + * Set the video mode (fps and resolution) of the camera + * @param mode the desired mode + */ + void setVideoMode(VideoMode mode); + + /** + * Set the video mode (fps and resolution) of the camera + * @param index the index of the desired mode + */ + void setVideoMode(int index); + + /** + * Set the gain of the camera + * NOTE - Not all cameras support this. + * @param gain the new gain to set the camera to + */ + void setGain(int gain); + + CameraCalibrationConfig getCurrentCalibrationData(); + List getAllCalibrationData(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java new file mode 100644 index 000000000..f4f4bb31d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java @@ -0,0 +1,103 @@ +package com.chameleonvision._2.vision.camera; + +import com.chameleonvision._2.vision.enums.StreamDivisor; +import com.chameleonvision._2.web.SocketHandler; +import edu.wpi.cscore.CvSource; +import edu.wpi.cscore.MjpegServer; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.cameraserver.CameraServer; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +public class CameraStreamer { + private final CameraCapture cameraCapture; + private final String name; + private StreamDivisor divisor; + private CvSource cvSource; + private final Object streamBufferLock = new Object(); + private Mat streamBuffer = new Mat(); + private Size size; + + public CameraStreamer(CameraCapture cameraCapture, String name, StreamDivisor div) { + this.divisor = div; + this.cameraCapture = cameraCapture; + this.name = name; + this.cvSource = CameraServer.getInstance().putVideo(name, + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value); + //noinspection IntegerDivisionInFloatingPointContext + this.size = new Size( + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value + ); + setDivisor(divisor, false); + } + + public void setDivisor(StreamDivisor newDivisor, boolean updateUI) { + this.divisor = newDivisor; + var camValues = cameraCapture.getProperties(); + var newWidth = camValues.getStaticProperties().imageWidth / newDivisor.value; + var newHeight = camValues.getStaticProperties().imageHeight / newDivisor.value; + this.size = new Size(newWidth, newHeight); + synchronized (streamBufferLock) { + this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3); + VideoMode oldVideoMode = cvSource.getVideoMode(); + cvSource.setVideoMode(new VideoMode(oldVideoMode.pixelFormat, + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value, + oldVideoMode.fps)); + } + if (updateUI) { + SocketHandler.sendFullSettings(); + } + + } + + public StreamDivisor getDivisor() { + return divisor; + } + + public void recalculateDivision() { + setDivisor(this.divisor, false); + } + + public void setNewVideoMode(VideoMode newVideoMode) { + // Trick to update cvSource and streamBuffer to the new resolution + // Must change the cameraProcess resolution first + setDivisor(divisor, true); + } + + public int getStreamPort() { + var s = (MjpegServer) CameraServer.getInstance().getServer("serve_" + name); + return s.getPort(); + } + + public void runStream(Mat image) { + synchronized (streamBufferLock) { + image.copyTo(streamBuffer); + } + + if (divisor.value != 1) { +// var camVal = cameraProcess.getProperties().staticProperties; +// var newWidth = camVal.imageWidth / divisor.value; +// var newHeight = camVal.imageHeight / divisor.value; +// Size newSize = new Size(newWidth, newHeight); + Imgproc.resize(streamBuffer, streamBuffer, this.size); + } + + var sourceVideoMode = cvSource.getVideoMode(); + var imageSize = streamBuffer.size(); + if(sourceVideoMode.width != (int) imageSize.width || sourceVideoMode.height != (int) imageSize.height) { + synchronized (streamBufferLock) { + cvSource.setVideoMode(new VideoMode(sourceVideoMode.pixelFormat, + (int)imageSize.width, + (int) imageSize.height, + sourceVideoMode.fps)); + } + } + + cvSource.putFrame(streamBuffer); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java new file mode 100644 index 000000000..20149c207 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java @@ -0,0 +1,39 @@ +package com.chameleonvision._2.vision.camera; + +import edu.wpi.cscore.VideoMode; +import org.apache.commons.math3.fraction.Fraction; +import org.apache.commons.math3.util.FastMath; + +public class CaptureStaticProperties { + + public final int imageWidth; + public final int imageHeight; + public final double fov; + public final double imageArea; + public final double centerX; + public final double centerY; + public final double horizontalFocalLength; + public final double verticalFocalLength; + public final VideoMode mode; + + public CaptureStaticProperties(VideoMode mode, double fov) { + this.mode = mode; + this.imageWidth = mode.width; + this.imageHeight = mode.height; + this.fov = fov; + imageArea = this.imageWidth * this.imageHeight; + centerX = ((double) this.imageWidth / 2) - 0.5; + centerY = ((double) this.imageHeight / 2) - 0.5; + + // pinhole model calculations + double diagonalView = FastMath.toRadians(this.fov); + Fraction aspectFraction = new Fraction(this.imageWidth, this.imageHeight); + int horizontalRatio = aspectFraction.getNumerator(); + int verticalRatio = aspectFraction.getDenominator(); + double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio); + double horizontalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2; + double verticalView = FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2; + horizontalFocalLength = this.imageWidth / (2 * FastMath.tan(horizontalView /2)); + verticalFocalLength = this.imageHeight / (2 * FastMath.tan(verticalView /2)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java new file mode 100644 index 000000000..937750219 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java @@ -0,0 +1,142 @@ +package com.chameleonvision._2.vision.camera; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.config.FullCameraConfiguration; +import com.chameleonvision._2.util.Helpers; +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.VideoException; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.cameraserver.CameraServer; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Size; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class USBCameraCapture implements CameraCapture { + private final UsbCamera baseCamera; + private final CvSink cvSink; + private List calibrationList; + private Mat imageBuffer = new Mat(); + private USBCaptureProperties properties; + + public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) { + var config = fullCameraConfiguration.cameraConfig; + this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration; + calibrationList.addAll(fullCameraConfiguration.calibration); + baseCamera = new UsbCamera(config.name, config.path); + cvSink = CameraServer.getInstance().getVideo(baseCamera); + try { + properties = new USBCaptureProperties(baseCamera, config); + } catch(VideoException e) { + System.err.println("Camera cannot be found on the saved USB port!" + + " Ensure that the camera has not been plugged into a different USB port, and if so, correct it."); + e.printStackTrace(); + } + + var videoModes = properties.getVideoModes(); + if(videoModes.size() < 1) { + throw new VideoException("0 video modes are valid! Full list provided by camera: \n\n" + + Arrays.stream(baseCamera.enumerateVideoModes()).map(Helpers::VideoModeToHashMap).toString() ); + } + + int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0; + setVideoMode(videoMode); + } + + public CameraCalibrationConfig getCalibration(Size size) { + for(var calibration: calibrationList) { + if(calibration.resolution.equals(size)) return calibration; + } + return null; + } + + public CameraCalibrationConfig getCalibration(VideoMode mode) { + return getCalibration(new Size(mode.width, mode.height)); + } + + public void addCalibrationData(CameraCalibrationConfig newConfig) { + calibrationList.removeIf(c -> newConfig.resolution.height == c.resolution.height && newConfig.resolution.width == c.resolution.width); + calibrationList.add(newConfig); + } + + @Override + public USBCaptureProperties getProperties() { + return properties; + } + + @Override + public VideoMode getCurrentVideoMode() { + return baseCamera.getVideoMode(); + } + + @Override + public Pair getFrame() { + Long deltaTime; + // TODO: Why multiply by 1000 here? + Mat tempMat = new Mat(); + deltaTime = cvSink.grabFrame(tempMat) * 1000L; +// tempMat = Imgcodecs.imread("C:\\Users\\imadu\\Documents\\GitHub\\chameleon-vision\\chameleon-server\\testimages\\2020\\image.png"); + tempMat.copyTo(imageBuffer); + tempMat.release(); + return Pair.of(imageBuffer, deltaTime); + } + + @Override + public void setExposure(int exposure) { + try { + baseCamera.setExposureManual(exposure); + } catch (VideoException e) { + System.err.println("Failed to change camera exposure!"); + } + } + + @Override + public void setBrightness(int brightness) { + try { + baseCamera.setBrightness(brightness); + } catch (VideoException e) { + System.err.println("Failed to change camera brightness!"); + } + } + + @Override + public void setVideoMode(VideoMode mode) { + try { + baseCamera.setVideoMode(mode); + properties.updateVideoMode(mode); + } catch (VideoException e) { + System.err.println("Failed to change camera video mode!"); + } + } + + public void setVideoMode(int index){ + VideoMode mode = properties.getVideoModes().get(index); + setVideoMode(mode); + } + + @Override + public void setGain(int gain) { + if (properties.isPS3Eye) { + try { + baseCamera.getProperty("gain_automatic").set(0); + baseCamera.getProperty("gain").set(gain); + } catch (Exception e) { + System.err.println("Failed to change camera gain!"); + } + } + } + + @Override + public CameraCalibrationConfig getCurrentCalibrationData() { + return getCalibration(getCurrentVideoMode()); + } + + @Override + public List getAllCalibrationData() { + return calibrationList; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java new file mode 100644 index 000000000..365870cbc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java @@ -0,0 +1,117 @@ +package com.chameleonvision._2.vision.camera; + +import com.chameleonvision._2.config.CameraJsonConfig; +import com.chameleonvision._2.vision.image.CaptureProperties; +import com.chameleonvision.common.util.Platform; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +import java.util.Arrays; +import java.util.List; +import java.util.function.Predicate; +import java.util.stream.Collectors; +import java.util.stream.Stream; + +public class USBCaptureProperties extends CaptureProperties { + public static final double DEFAULT_FOV = 70; + private static final int DEFAULT_EXPOSURE = 50; + private static final int DEFAULT_BRIGHTNESS = 50; + private static final int MINIMUM_FPS = 21; + private static final int MINIMUM_WIDTH = 320; + private static final int MINIMUM_HEIGHT = 200; + private static final int MAX_INIT_MS = 1500; + + private static final int PS3EYE_VID = 0x1415; + private static final int PS3EYE_PID = 0x2000; + + private static final List ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG, VideoMode.PixelFormat.kBGR); + + private static final Predicate kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS); + private static final Predicate kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT); + private static final Predicate kPixelFormatPredicate = (videoMode -> ALLOWED_PIXEL_FORMATS.contains(videoMode.pixelFormat)); + + public final String name; + public final String path; + public final List videoModes; + + private final UsbCamera baseCamera; + public final boolean isPS3Eye; + + private String nickname; + private double FOV; + + USBCaptureProperties(UsbCamera baseCamera, CameraJsonConfig config) { + FOV = config.fov; + name = config.name; + path = config.path; + setTilt(Rotation2d.fromDegrees(config.tilt)); + nickname = config.nickname; + this.baseCamera = baseCamera; + + int usbVID = baseCamera.getInfo().vendorId; + int usbPID = baseCamera.getInfo().productId; + + // wait for camera USB init on Windows, Windows USB is slow... + if (Platform.CurrentPlatform == Platform.WINDOWS_64 && !baseCamera.isConnected()) { + System.out.print("Waiting on camera... "); + long initTimeout = System.nanoTime(); + while (!baseCamera.isConnected()) { + if (((System.nanoTime() - initTimeout) / 1e6) >= MAX_INIT_MS) { + break; + } + } + var initTimeMs = (System.nanoTime() - initTimeout) / 1e6; + System.out.printf("USBCameraProcess initialized in %.2fms\n", initTimeMs); + } + + isPS3Eye = (usbVID == PS3EYE_VID && usbPID == PS3EYE_PID); + videoModes = filterVideoModes(baseCamera.enumerateVideoModes()); + } + + public void setFOV(double FOV) { + if (this.FOV != FOV) { + this.FOV = FOV; + staticProperties = new CaptureStaticProperties(staticProperties.mode, FOV); + } + } + + public double getFOV() { + return FOV; + } + + public void setNickname(String nickname) { + this.nickname = nickname; + } + + public String getNickname() { + return nickname; + } + + private List filterVideoModes(VideoMode[] videoModes) { + Predicate fullPredicate = kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate); + Stream validModes = Arrays.stream(videoModes).filter(fullPredicate); + return validModes.collect(Collectors.toList()); + } + + void updateVideoMode(VideoMode videoMode) { + staticProperties = new CaptureStaticProperties(videoMode, FOV); + } + + public List getVideoModes() { + return videoModes; + } + + public VideoMode getVideoMode(int index){ + return videoModes.get(index); + } + + public VideoMode getCurrentVideoMode() { return staticProperties.mode; } + + public int getCurrentVideoModeIndex(){ + return getVideoModes().indexOf(getCurrentVideoMode()); + } + + + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java new file mode 100644 index 000000000..71d62d17e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java @@ -0,0 +1,5 @@ +package com.chameleonvision._2.vision.enums; + +public enum CalibrationMode { + None,Single,Dual +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageFlipMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageFlipMode.java new file mode 100644 index 000000000..af65aa3c7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageFlipMode.java @@ -0,0 +1,14 @@ +package com.chameleonvision._2.vision.enums; + +public enum ImageFlipMode { + NONE(Integer.MIN_VALUE), + VERTICAL(1), + HORIZONTAL(0), + BOTH(-1); + + public final int value; + + ImageFlipMode(int value) { + this.value = value; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java new file mode 100644 index 000000000..7328dc02d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java @@ -0,0 +1,16 @@ +package com.chameleonvision._2.vision.enums; + +public enum ImageRotationMode { + DEG_0(-1), + DEG_90(0), + DEG_180(1), + DEG_270(2); + + public final int value; + + ImageRotationMode(int value) { + this.value = value; + } + + public boolean isRotated(){return this.value==DEG_90.value||this.value==DEG_270.value;} +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java new file mode 100644 index 000000000..db7acefbe --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java @@ -0,0 +1,5 @@ +package com.chameleonvision._2.vision.enums; + +public enum SortMode { + Largest,Smallest,Highest,Lowest,Rightmost,Leftmost,Centermost +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/StreamDivisor.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/StreamDivisor.java new file mode 100644 index 000000000..a787e1ee5 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/StreamDivisor.java @@ -0,0 +1,14 @@ +package com.chameleonvision._2.vision.enums; + +public enum StreamDivisor { + NONE(1), + HALF(2), + QUARTER(4), + SIXTH(6); + + public final Integer value; + + StreamDivisor(int value) { + this.value = value; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetGroup.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetGroup.java new file mode 100644 index 000000000..67e3e6653 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetGroup.java @@ -0,0 +1,6 @@ +package com.chameleonvision._2.vision.enums; + +public enum TargetGroup { + Single, + Dual +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java new file mode 100644 index 000000000..8e754faf9 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java @@ -0,0 +1,5 @@ +package com.chameleonvision._2.vision.enums; + +public enum TargetIntersection { + None,Up,Down,Left,Right +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java new file mode 100644 index 000000000..8276ecb1d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java @@ -0,0 +1,5 @@ +package com.chameleonvision._2.vision.enums; + +public enum TargetOrientation { + Portrait, Landscape +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java new file mode 100644 index 000000000..743046b4b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java @@ -0,0 +1,5 @@ +package com.chameleonvision._2.vision.enums; + +public enum TargetRegion { + Center, Top, Bottom, Left, Right +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java new file mode 100644 index 000000000..816f927da --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java @@ -0,0 +1,30 @@ +package com.chameleonvision._2.vision.image; + +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +public class CaptureProperties { + + protected CaptureStaticProperties staticProperties; + private Rotation2d tilt = new Rotation2d(); + + protected CaptureProperties() { + } + + public CaptureProperties(VideoMode videoMode, double fov) { + staticProperties = new CaptureStaticProperties(videoMode, fov); + } + public void setStaticProperties(CaptureStaticProperties staticProperties) {this.staticProperties = staticProperties;} + public CaptureStaticProperties getStaticProperties() { + return staticProperties; + } + + public Rotation2d getTilt() { + return tilt; + } + + public void setTilt(Rotation2d tilt) { + this.tilt = tilt; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java new file mode 100644 index 000000000..cd8e3a4d2 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java @@ -0,0 +1,12 @@ +package com.chameleonvision._2.vision.image; + +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; + +public interface ImageCapture { + /** + * Get the next camera frame + * @return a Pair of the captured image and the Linux epoch of when the frame was grabbed (in uS) + */ + Pair getFrame(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java new file mode 100644 index 000000000..e10d51e63 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java @@ -0,0 +1,87 @@ +package com.chameleonvision._2.vision.image; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.vision.camera.CameraCapture; +import edu.wpi.cscore.VideoMode; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.imgcodecs.Imgcodecs; + +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.List; + +public class StaticImageCapture implements CameraCapture { + + private Mat image = new Mat(); + private final VideoMode fakeVideoMode; + private final CaptureProperties properties; + + public StaticImageCapture(Path imagePath) { + this(imagePath, 70); + } + + public StaticImageCapture(Path imagePath, double FOV) { + if (!Files.exists(imagePath)) throw new RuntimeException("Invalid path for image!"); + + Mat loadedImage = Imgcodecs.imread(imagePath.toString()); + loadedImage.copyTo(image); + if (image.cols() > 0 && image.rows() > 0) { + fakeVideoMode = new VideoMode(VideoMode.PixelFormat.kBGR, image.cols(), image.rows(), 60); + } else { + throw new RuntimeException("Failed to load image!"); + } + + properties = new CaptureProperties(fakeVideoMode, FOV); + } + + @Override + public Pair getFrame() { + return Pair.of(image, System.nanoTime()); + } + + @Override + public CaptureProperties getProperties() { + return properties; + } + + @Override + public VideoMode getCurrentVideoMode() { + return fakeVideoMode; + } + + @Override + public void setExposure(int exposure) { + // do nothing + } + + @Override + public void setBrightness(int brightness) { + // do nothing + } + + @Override + public void setVideoMode(VideoMode mode) { + // do nothing + } + + @Override + public void setVideoMode(int index) { + // do nothing + } + + @Override + public void setGain(int gain) { + // do nothing + } + + @Override + public CameraCalibrationConfig getCurrentCalibrationData() { + return null; + } + + @Override + public List getAllCalibrationData() { + return null; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java new file mode 100644 index 000000000..ccac6cbfc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java @@ -0,0 +1,32 @@ +package com.chameleonvision._2.vision.pipeline; + +import com.chameleonvision._2.vision.camera.CameraCapture; +import org.opencv.core.Mat; + +/** + * + * @param Pipeline result type + */ +public abstract class CVPipeline { + protected Mat outputMat = new Mat(); + protected CameraCapture cameraCapture; + public S settings; + + protected CVPipeline(S settings) { + this.settings = settings; + } + + protected CVPipeline(String pipelineName, S settings) { + this.settings = settings; + settings.nickname = pipelineName; + } + + public void initPipeline(CameraCapture camera) { + cameraCapture = camera; + cameraCapture.setVideoMode(settings.videoModeIndex); + cameraCapture.setExposure((int) settings.exposure); + cameraCapture.setBrightness((int) settings.brightness); + cameraCapture.setGain((int) settings.gain); + } + abstract public R runPipeline(Mat inputMat); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineResult.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineResult.java new file mode 100644 index 000000000..90368951a --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineResult.java @@ -0,0 +1,26 @@ +package com.chameleonvision._2.vision.pipeline; + +import org.opencv.core.Mat; + +import java.util.List; + +public abstract class CVPipelineResult { + public final List targets; + public final boolean hasTarget; + public final Mat outputMat = new Mat(); + public final long processTime; + public long imageTimestamp = 0; + + public CVPipelineResult(List targets, Mat outputMat, long processTime) { + this.targets = targets; + hasTarget = targets != null && !targets.isEmpty(); +// this.outputMat = outputMat; + outputMat.copyTo(this.outputMat); + outputMat.release(); + this.processTime = processTime; + } + + public void setTimestamp(long timestamp) { + imageTimestamp = timestamp; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineSettings.java new file mode 100644 index 000000000..f21a12a90 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipelineSettings.java @@ -0,0 +1,18 @@ +package com.chameleonvision._2.vision.pipeline; + +import com.chameleonvision._2.vision.enums.ImageFlipMode; +import com.chameleonvision._2.vision.enums.ImageRotationMode; +import com.chameleonvision._2.vision.enums.StreamDivisor; + +@SuppressWarnings("ALL") +public class CVPipelineSettings { + public int index = 0; + public ImageFlipMode flipMode = ImageFlipMode.NONE; + public ImageRotationMode rotationMode = ImageRotationMode.DEG_0; + public String nickname = "New Pipeline"; + public double exposure = 50.0; + public double brightness = 50.0; + public double gain = 0; + public int videoModeIndex = 0; + public StreamDivisor streamDivisor = StreamDivisor.NONE; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java new file mode 100644 index 000000000..0cb962d0b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java @@ -0,0 +1,13 @@ +package com.chameleonvision._2.vision.pipeline; + +import org.apache.commons.lang3.tuple.Pair; + +public interface Pipe { + /** + * + * @param input Input object for pipe + * @return Returns a Pair containing the process time in Nanoseconds, + * and the output object + */ + Pair run(I input); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java new file mode 100644 index 000000000..f75b89357 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java @@ -0,0 +1,251 @@ +package com.chameleonvision._2.vision.pipeline; + +import com.chameleonvision._2.config.CameraConfig; +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.vision.VisionManager; +import com.chameleonvision._2.vision.VisionProcess; +import com.chameleonvision._2.vision.pipeline.impl.Calibrate3dPipeline; +import com.chameleonvision._2.vision.pipeline.impl.DriverVisionPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.chameleonvision._2.web.SocketHandler; +import com.chameleonvision.common.scripting.ScriptEventType; +import com.chameleonvision.common.scripting.ScriptManager; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.networktables.NetworkTableEntry; + +import java.util.Comparator; +import java.util.HashMap; +import java.util.LinkedList; +import java.util.List; + +@SuppressWarnings("WeakerAccess") +public class PipelineManager { + + private static final int DRIVERMODE_INDEX = -1; + private static final int CAL_3D_INDEX = -2; + + public final LinkedList pipelines = new LinkedList<>(); + + public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings()); + public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings()); + + private final VisionProcess parentProcess; + private int lastPipelineIndex; + private int currentPipelineIndex; + public NetworkTableEntry ntIndexEntry; + + public PipelineManager(VisionProcess visionProcess, List loadedPipelineSettings) { + parentProcess = visionProcess; + if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) { + pipelines.add(new StandardCVPipeline("New Pipeline")); + } else { + for (CVPipelineSettings setting : loadedPipelineSettings) { + addInternalPipeline(setting); + } + } + driverModePipeline.initPipeline(visionProcess.getCamera()); + setCurrentPipeline(0); + } + + private void reassignIndexes() { + pipelines.sort(IndexComparator); + for (int i = 0; i < pipelines.size(); i++) { + pipelines.get(i).settings.index = i; + } + } + + private CameraConfig getConfig(VisionProcess process) { + return VisionManager.getCameraConfig(process); + } + + private CameraConfig getConfig() { + return getConfig(parentProcess); + } + + private void savePipelineConfig(CVPipelineSettings setting) { + getConfig().pipelineConfig.save(setting); + } + + private void deletePipelineConfig(CVPipelineSettings setting) { + getConfig().pipelineConfig.delete(setting); + } + + private void renamePipelineConfig(CVPipelineSettings setting, String newName) { + getConfig().pipelineConfig.rename(setting, newName); + } + + public void saveAllPipelines() { + pipelines.parallelStream().map(pipeline -> pipeline.settings).forEach(this::savePipelineConfig); + } + + private void addInternalPipeline(CVPipelineSettings setting) { + if (setting instanceof StandardCVPipelineSettings) { + pipelines.add(new StandardCVPipeline((StandardCVPipelineSettings) setting)); + } else { + System.out.println("Non 2D/3D pipelines not supported!"); + } + reassignIndexes(); + } + + public void setDriverMode(boolean driverMode) { + if (driverMode) setCurrentPipeline(DRIVERMODE_INDEX); + else setCurrentPipeline(lastPipelineIndex); + } + + public void setCalibrationMode(boolean calibrationMode) { + setCurrentPipeline((calibrationMode ? CAL_3D_INDEX : lastPipelineIndex)); + } + + public void enableCalibrationMode(VideoMode mode) { + parentProcess.setVideoMode(mode); + calib3dPipe.setVideoMode(mode); + setCalibrationMode(true); + } + + public boolean getDriverMode() { + return currentPipelineIndex == DRIVERMODE_INDEX; + } + + public int getCurrentPipelineIndex() { + return currentPipelineIndex; + } + + public CVPipeline getCurrentPipeline() { + if (currentPipelineIndex == DRIVERMODE_INDEX) { + return driverModePipeline; + } else if (currentPipelineIndex <= CAL_3D_INDEX) { + return calib3dPipe; + } else { + return pipelines.get(currentPipelineIndex); + } + } + + public void setCurrentPipeline(int index) { + CVPipeline newPipeline = null; + + if (index == DRIVERMODE_INDEX) { + ScriptManager.queueEvent(ScriptEventType.kLEDOff); + newPipeline = driverModePipeline; + + // if we're changing into driver mode, try to set the nt entry to true + parentProcess.setDriverModeEntry(true); + } else if (index == CAL_3D_INDEX) { + parentProcess.setDriverModeEntry(true); + + newPipeline = calib3dPipe; + } else { + if (index < pipelines.size()&&index>=0) { + newPipeline = pipelines.get(index); + + // if we're switching out of driver mode, try to set the nt entry to false + parentProcess.setDriverModeEntry(false); + ScriptManager.queueEvent(ScriptEventType.kLEDOn); + } + else + { + //TODO alert/warn user that pipeline doesnt exsits + System.err.println("Index is out of bounds"); + } + } + if (newPipeline != null) { + lastPipelineIndex = currentPipelineIndex; + currentPipelineIndex = index; + getCurrentPipeline().initPipeline(parentProcess.getCamera()); + + if (ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) { + ConfigManager.settings.currentPipeline = currentPipelineIndex; + + HashMap pipeChange = new HashMap<>(); + pipeChange.put("currentPipeline", currentPipelineIndex); + SocketHandler.broadcastMessage(pipeChange); + try { + SocketHandler.sendFullSettings(); + } catch (Exception e) { + // avoid NullPointerException when run before threads start + } + } + newPipeline.initPipeline(parentProcess.getCamera()); + if (parentProcess.cameraStreamer != null) + parentProcess.cameraStreamer.setDivisor(newPipeline.settings.streamDivisor, true); + if (ntIndexEntry != null) { + ntIndexEntry.setDouble(index); + } + } + + // gain setting quirk + if (!parentProcess.cameraCapture.getProperties().isPS3Eye) { + getCurrentPipeline().settings.gain = -1; + } + } + + public void addPipeline(CVPipelineSettings setting) { + addInternalPipeline(setting); + savePipelineConfig(setting); + } + + public void addPipeline(CVPipeline pipeline) { + pipelines.add(pipeline); + reassignIndexes(); + savePipelineConfig(pipeline.settings); + } + + public void addNewPipeline(String piplineName) { + StandardCVPipeline newPipeline = new StandardCVPipeline(); + newPipeline.settings.nickname = piplineName; + newPipeline.settings.index = pipelines.size(); + addPipeline(newPipeline); + } + + public CVPipeline getPipeline(int index) { + return pipelines.get(index); + } + + public void duplicatePipeline(CVPipelineSettings pipeline) { + duplicatePipeline(pipeline, parentProcess); + } + + public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) { + pipeline.index = destinationProcess.pipelineManager.pipelines.size(); + pipeline.nickname += "(Copy)"; + if (destinationProcess.pipelineManager.pipelines.stream().anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))){ +// throw new DuplicatedKeyException("key Already exists"); + } else{ + destinationProcess.pipelineManager.addPipeline(pipeline); + } + } + + public void renameCurrentPipeline(String newName) { + CVPipelineSettings settings = getCurrentPipeline().settings; + renamePipelineConfig(settings, newName); + } + + public void deleteCurrentPipeline() { + deletePipeline(currentPipelineIndex); + } + + private void deletePipeline(int index) { + if (index == currentPipelineIndex) { + currentPipelineIndex -= 1; + } + deletePipelineConfig(getPipeline(index).settings); + pipelines.remove(index); + reassignIndexes(); + } + + public void saveDriverModeConfig() { + getConfig().saveDriverMode(driverModePipeline.settings); + } + + private static final Comparator IndexComparator = (o1, o2) -> { + int o1Index = o1.settings.index; + int o2Index = o2.settings.index; + + if (o1Index == o2Index) { + return 0; + } else if (o1Index < o2Index) { + return -1; + } + return 1; + }; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java new file mode 100644 index 000000000..6c474d595 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java @@ -0,0 +1,170 @@ +package com.chameleonvision._2.vision.pipeline.impl; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.vision.pipeline.CVPipeline; +import com.chameleonvision._2.vision.VisionManager; +import com.chameleonvision._2.vision.camera.CameraCapture; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.wpilibj.util.Units; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; + +import java.util.ArrayList; +import java.util.List; + +public class Calibrate3dPipeline extends CVPipeline { + + private int checkerboardSquaresHigh = 7; + private int checkerboardSquaresWide = 7; + + private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3); + private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide); + private Size imageSize; + double checkerboardSquareSize = 1; // inches! + private MatOfPoint2f calibrationOutput = new MatOfPoint2f(); + private List objpoints = new ArrayList<>(); + private List imgpoints = new ArrayList<>(); + + public static double checkerboardSquareSizeUnits = Units.inchesToMeters(1.0); + + public static final int MIN_COUNT = 15; + private VideoMode calibrationMode; + private final Size windowSize = new Size(11, 11); + private final Size zeroZone = new Size(-1, -1); + private TermCriteria criteria = new TermCriteria(3, 30, 0.001); //(Imgproc.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + private int captureCount = 0; + private double calibrationAccuracy = 0; + private boolean wantsSnapshot = false; + private double squareSizeInches; + + public Calibrate3dPipeline(StandardCVPipelineSettings settings) { + super(settings); + + objP = new MatOfPoint3f(); + + for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) { + objP.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f))); + } + + setSquareSize(checkerboardSquareSizeUnits); + + objpoints.forEach(Mat::release); + imgpoints.forEach(Mat::release); + objpoints.clear(); + imgpoints.clear(); + } + + public void setSquareSize(double size) { + this.squareSizeInches = size; + } + + public void takeSnapshot() { + wantsSnapshot = true; + } + + public boolean hasEnoughSnapshots() { + return captureCount >= MIN_COUNT - 1; + } + + @Override + public DriverVisionPipeline.DriverPipelineResult runPipeline(Mat inputMat) { + + // look for checkerboard + Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY); + var checkerboardFound = Calib3d.findChessboardCorners(inputMat, patternSize, calibrationOutput); + + + + if(!checkerboardFound) { + Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR); + + return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0); + } + +// System.out.println("[SolvePNP] checkerboard found!!"); + + // cool we found a checkerboard + // do corner subpixel + Imgproc.cornerSubPix(inputMat, calibrationOutput, windowSize, zeroZone, criteria); + + // convert back to BGR + Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR); + // draw the chessboard + Calib3d.drawChessboardCorners(inputMat, patternSize, calibrationOutput, true); + + if(wantsSnapshot) { + this.imageSize = new Size(inputMat.width(), inputMat.height()); + + var mat = new MatOfPoint3f(); + calibrationOutput.copyTo(mat); + this.objpoints.add(objP); + imgpoints.add(mat); + captureCount++; + wantsSnapshot = false; + } + + imageSize = new Size(inputMat.width(), inputMat.height()); + + return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0); + } + + @Override + public void initPipeline(CameraCapture camera) { + super.initPipeline(camera); + objpoints.clear(); + imgpoints.clear(); + captureCount = 0; + } + + public boolean tryCalibration() { + if (!hasEnoughSnapshots()) return false; + + Mat cameraMatrix = new Mat(); + Mat distortionCoeffs = new Mat(); + List rvecs = new ArrayList<>(); + List tvecs = new ArrayList<>(); + + try { + calibrationAccuracy = Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs); + } catch(Exception e) { + System.err.println("Camera calibration failed!"); + initPipeline(cameraCapture); + return false; + } + + VideoMode currentVidMode = cameraCapture.getCurrentVideoMode(); + Size resolution = new Size(currentVidMode.width, currentVidMode.height); + CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs, squareSizeInches); + + VisionManager.getCurrentUIVisionProcess().addCalibration(cal); + + try { + System.out.printf("CALIBRATION SUCCESS (with accuracy %s)! camMatrix: \n%s\ndistortionCoeffs:\n%s\n", + calibrationAccuracy, new ObjectMapper().writeValueAsString(cal.cameraMatrix), new ObjectMapper().writeValueAsString(cal.distortionCoeffs)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + + + ConfigManager.saveGeneralSettings(); + + return true; + } + + public void setVideoMode(VideoMode mode){ + this.calibrationMode = mode; + } + + public int getSnapshotCount() { + return captureCount + 1; + } + + public double getCalibrationAccuracy(){ + return calibrationAccuracy; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/DriverVisionPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/DriverVisionPipeline.java new file mode 100644 index 000000000..f6f00e79b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/DriverVisionPipeline.java @@ -0,0 +1,55 @@ +package com.chameleonvision._2.vision.pipeline.impl; + +import com.chameleonvision._2.vision.camera.CameraCapture; +import com.chameleonvision._2.vision.enums.CalibrationMode; +import com.chameleonvision._2.vision.pipeline.CVPipeline; +import com.chameleonvision._2.vision.pipeline.CVPipelineResult; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision._2.vision.pipeline.pipes.Draw2dCrosshairPipe; +import com.chameleonvision._2.vision.pipeline.pipes.RotateFlipPipe; +import com.chameleonvision.common.util.MemoryManager; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; + +import java.util.List; + + +public class DriverVisionPipeline extends CVPipeline { + + private RotateFlipPipe rotateFlipPipe; + private Draw2dCrosshairPipe drawCrosshairPipe; + private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings(); + + private final MemoryManager memoryManager = new MemoryManager(200, 20000); + + public DriverVisionPipeline(CVPipelineSettings settings) { + super(settings); + settings.index = -1; + } + + @Override + public void initPipeline(CameraCapture capture) { + super.initPipeline(capture); + rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode); + crosshairPipeSettings.showCrosshair=true; + drawCrosshairPipe = new Draw2dCrosshairPipe(crosshairPipeSettings, CalibrationMode.None,null,0,0); + } + + @Override + public DriverPipelineResult runPipeline(Mat inputMat) { + + rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode); + + Pair rotateFlipResult = rotateFlipPipe.run(inputMat); + Pair draw2dCrosshairResult = drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(),null)); + memoryManager.run(); + + return new DriverPipelineResult(null, draw2dCrosshairResult.getLeft(), 0); + } + + public static class DriverPipelineResult extends CVPipelineResult { + public DriverPipelineResult(List targets, Mat outputMat, long processTime) { + super(targets, outputMat, processTime); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java new file mode 100644 index 000000000..b8e934d42 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java @@ -0,0 +1,284 @@ +package com.chameleonvision._2.vision.pipeline.impl; + +import com.chameleonvision._2.Main; +import com.chameleonvision._2.vision.camera.CameraCapture; +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.pipeline.CVPipeline; +import com.chameleonvision._2.vision.pipeline.CVPipelineResult; +import com.chameleonvision._2.vision.pipeline.pipes.*; +import com.chameleonvision.common.util.MemoryManager; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Point; +import org.opencv.core.*; + +import java.awt.*; +import java.util.List; + +@SuppressWarnings("WeakerAccess") +public class StandardCVPipeline extends CVPipeline { + + private Mat rawCameraMat = new Mat(); + + private RotateFlipPipe rotateFlipPipe; + private BlurPipe blurPipe; + private ErodeDilatePipe erodeDilatePipe; + private HsvPipe hsvPipe; + private FindContoursPipe findContoursPipe; + private FilterContoursPipe filterContoursPipe; + private SpeckleRejectPipe speckleRejectPipe; + private GroupContoursPipe groupContoursPipe; + private SortContoursPipe sortContoursPipe; + private Collect2dTargetsPipe collect2dTargetsPipe; + private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings; + private Draw2dContoursPipe draw2dContoursPipe; + private Draw2dCrosshairPipe draw2dCrosshairPipe; + private DrawSolvePNPPipe drawSolvePNPPipe; + private SolvePNPPipe solvePNPPipe; + private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings draw2dCrosshairPipeSettings; + private OutputMatPipe outputMatPipe; + + private String pipelineTimeString = ""; + private CaptureStaticProperties camProps; + private Scalar hsvLower, hsvUpper; + + public StandardCVPipeline() { + super(new StandardCVPipelineSettings()); + } + + public StandardCVPipeline(String name) { + super(name, new StandardCVPipelineSettings()); + } + + public StandardCVPipeline(StandardCVPipelineSettings settings) { + super(settings); + } + + @Override + public void initPipeline(CameraCapture process) { + super.initPipeline(process); + + camProps = cameraCapture.getProperties().getStaticProperties(); + hsvLower = new Scalar(settings.hue.getFirst(), settings.saturation.getFirst(), settings.value.getFirst()); + hsvUpper = new Scalar(settings.hue.getSecond(), settings.saturation.getSecond(), settings.value.getSecond()); + + rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode); + blurPipe = new BlurPipe(5); + erodeDilatePipe = new ErodeDilatePipe(settings.erode, settings.dilate, 7); + hsvPipe = new HsvPipe(hsvLower, hsvUpper); + findContoursPipe = new FindContoursPipe(); + filterContoursPipe = new FilterContoursPipe(settings.area, settings.ratio, settings.extent, camProps); + speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue()); + groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection); + sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps, 5); + collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps); + draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings(); + draw2dCrosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings(); + + draw2dContoursSettings.showCentroid = true; + draw2dContoursSettings.centroidColor = new Color(25, 239, 0); + draw2dContoursSettings.boxOutlineSize = 2; + draw2dContoursSettings.showRotatedBox = true; + draw2dContoursSettings.showMaximumBox = true; + draw2dContoursSettings.showMultiple = settings.multiple; + draw2dCrosshairPipeSettings.showCrosshair = true; + draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps); + draw2dCrosshairPipe = new Draw2dCrosshairPipe(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB); + outputMatPipe = new OutputMatPipe(settings.isBinary); + } + + private final MemoryManager memManager = new MemoryManager(120, 20000); + + private StandardCVPipelineResult resultCache = new StandardCVPipelineResult(List.of(), new Mat(), 0L); + + @Override + public StandardCVPipelineResult runPipeline(Mat inputMat) { + long totalPipelineTimeNanos = 0; + long pipelineStartTimeNanos = System.nanoTime(); + + resultCache.release(); + + if (cameraCapture == null) { + throw new RuntimeException("Pipeline was not initialized before being run!"); + } + + // TODO (HIGH) find the source of the random NPE + if (settings == null) { + throw new RuntimeException("settings was not initialized!"); + } + if (inputMat.cols() <= 1) { + throw new RuntimeException("Input Mat is empty!"); + } + + pipelineTimeString = ""; + + // prepare pipes + camProps = cameraCapture.getProperties().getStaticProperties(); + hsvLower = new Scalar(settings.hue.getFirst(), settings.saturation.getFirst(), settings.value.getFirst()); + hsvUpper = new Scalar(settings.hue.getSecond(), settings.saturation.getSecond(), settings.value.getSecond()); + rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode); + blurPipe.setConfig(0); + erodeDilatePipe.setConfig(settings.erode, settings.dilate, 7); + hsvPipe.setConfig(hsvLower, hsvUpper); + filterContoursPipe.setConfig(settings.area, settings.ratio, settings.extent, camProps); + speckleRejectPipe.setConfig(settings.speckle.doubleValue()); + groupContoursPipe.setConfig(settings.targetGroup, settings.targetIntersection); + sortContoursPipe.setConfig(settings.sortMode, camProps, 5); + collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.targetRegion, settings.targetOrientation, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps); + draw2dContoursPipe.setConfig(settings.multiple, camProps); + draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings, settings.calibrationMode, settings.point, settings.dualTargetCalibrationM, settings.dualTargetCalibrationB); + outputMatPipe.setConfig(settings.isBinary); + + if (settings.is3D) { + if (solvePNPPipe == null) + solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt()); + if (drawSolvePNPPipe == null) + drawSolvePNPPipe = new DrawSolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData()); + + solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt()); + drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData()); + drawSolvePNPPipe.setConfig(settings); + + } + + long pipeInitTimeNanos = System.nanoTime() - pipelineStartTimeNanos; + + // run pipes + Pair rotateFlipResult = rotateFlipPipe.run(inputMat); + totalPipelineTimeNanos += rotateFlipResult.getRight(); + + inputMat.copyTo(rawCameraMat); + +// Pair blurResult = blurPipe.run(rotateFlipResult.getLeft()); +// totalPipelineTimeNanos += blurResult.getRight(); + + Pair erodeDilateResult = erodeDilatePipe.run(rotateFlipResult.getLeft()); + totalPipelineTimeNanos += erodeDilateResult.getRight(); + + Pair hsvResult = hsvPipe.run(erodeDilateResult.getLeft()); + totalPipelineTimeNanos += hsvResult.getRight(); + + Pair, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft()); + totalPipelineTimeNanos += findContoursResult.getRight(); + + Pair, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft()); + totalPipelineTimeNanos += filterContoursResult.getRight(); + + Pair, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft()); + totalPipelineTimeNanos += speckleRejectResult.getRight(); + + Pair, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft()); + totalPipelineTimeNanos += groupContoursResult.getRight(); + + Pair, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft()); + totalPipelineTimeNanos += sortContoursResult.getRight(); + + Pair, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps)); + totalPipelineTimeNanos += collect2dTargetsResult.getRight(); + + // takes pair of (Mat of original camera image (8UC3), Mat of HSV thresholded image(8UC1)) + Pair outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvResult.getLeft())); + totalPipelineTimeNanos += outputMatResult.getRight(); + + Pair result; + + if (!settings.is3D) { + // takes pair of (Mat to draw on, List of sorted contours) + result = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft())); + totalPipelineTimeNanos += result.getRight(); + } else { + result = outputMatResult; + } + + // takes pair of (Mat to draw on, List of sorted contours) + Pair draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(), collect2dTargetsResult.getLeft())); + totalPipelineTimeNanos += draw2dCrosshairResult.getRight(); + + Mat outputMat; + + if (settings.is3D) { + // once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe + Pair, Long> solvePNPResult = solvePNPPipe.run(Pair.of(collect2dTargetsResult.getLeft(), rotateFlipResult.getLeft())); + totalPipelineTimeNanos += solvePNPResult.getRight(); + + Pair draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft())); + totalPipelineTimeNanos += draw3dContoursResult.getRight(); + + outputMat = draw3dContoursResult.getLeft(); + } else { + outputMat = draw2dCrosshairResult.getLeft(); + } + + if (Main.testMode) { + pipelineTimeString += String.format("PipeInit: %.2fms, ", pipeInitTimeNanos / 1000000.0); + pipelineTimeString += String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000000.0); +// pipelineTimeString += String.format("Blur: %.2fms, ", blurResult.getRight() / 1000000.0); + pipelineTimeString += String.format("ErodeDilate: %.2fms, ", erodeDilateResult.getRight() / 1000000.0); + pipelineTimeString += String.format("HSV: %.2fms, ", hsvResult.getRight() / 1000000.0); + pipelineTimeString += String.format("FindContours: %.2fms, ", findContoursResult.getRight() / 1000000.0); + pipelineTimeString += String.format("FilterContours: %.2fms, ", filterContoursResult.getRight() / 1000000.0); + pipelineTimeString += String.format("SpeckleReject: %.2fms, ", speckleRejectResult.getRight() / 1000000.0); + pipelineTimeString += String.format("GroupContours: %.2fms, ", groupContoursResult.getRight() / 1000000.0); + pipelineTimeString += String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000000.0); + pipelineTimeString += String.format("Collect2dTargets: %.2fms, ", collect2dTargetsResult.getRight() / 1000000.0); + pipelineTimeString += String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000000.0); + pipelineTimeString += String.format("Draw2dContours: %.2fms, ", result.getRight() / 1000000.0); + pipelineTimeString += String.format("Draw2dCrosshair: %.2fms, ", draw2dCrosshairResult.getRight() / 1000000.0); + + System.out.println(pipelineTimeString); + double totalPipelineTimeMillis = totalPipelineTimeNanos / 1000000.0; + double totalPipelineTimeFPS = 1.0 / (totalPipelineTimeMillis / 1000.0); + double truePipelineTimeMillis = (System.nanoTime() - pipelineStartTimeNanos) / 1000000.0; + double truePipelineFPS = 1.0 / (truePipelineTimeMillis / 1000.0); + System.out.printf("Pipeline processed in %.3fms (%.2fFPS), ", totalPipelineTimeMillis, totalPipelineTimeFPS); + System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS); + } + +// memManager.run(); + + resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos); + return resultCache; + } + + public static class StandardCVPipelineResult extends CVPipelineResult { + public StandardCVPipelineResult(List targets, Mat outputMat, long processTimeNanos) { + super(targets, outputMat, processTimeNanos); + } + + public void release() { + targets.forEach(TrackedTarget::release); + outputMat.release(); + } + } + + public static class TrackedTarget { + public double calibratedX = 0.0; + public double calibratedY = 0.0; + public double pitch = 0.0; + public double yaw = 0.0; + public double area = 0.0; + public Point point = new Point(); + public RotatedRect minAreaRect; + public Rect boundingRect; + + // 3d stuff + public Pose2d cameraRelativePose = new Pose2d(); + public Mat rVector = new Mat(); + public Mat tVector = new Mat(); + public MatOfPoint2f imageCornerPoints = new MatOfPoint2f(); + public Pair leftRightDualTargetPair = null; + public Pair leftRightRotatedRect = null; + public MatOfPoint2f rawContour = kMat2f; + public MatOfPoint2f approxPoly = new MatOfPoint2f(); + + public void release() { + rVector.release(); + tVector.release(); + imageCornerPoints.release(); + } + + private static final MatOfPoint2f kMat2f = new MatOfPoint2f(); + } + + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java new file mode 100644 index 000000000..6a827983d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java @@ -0,0 +1,49 @@ +package com.chameleonvision._2.vision.pipeline.impl; + +import com.chameleonvision._2.vision.enums.*; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.util.numbers.IntegerCouple; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; + +public class StandardCVPipelineSettings extends CVPipelineSettings { + public IntegerCouple hue = new IntegerCouple(50, 180); + public IntegerCouple saturation = new IntegerCouple(50, 255); + public IntegerCouple value = new IntegerCouple(50, 255); + public boolean erode = false; + public boolean dilate = false; + public DoubleCouple area = new DoubleCouple(0.0, 100.0); + public DoubleCouple ratio = new DoubleCouple(0.0, 20.0); + public DoubleCouple extent = new DoubleCouple(0.0, 100.0); + public Number speckle = 5; + public boolean isBinary = false; + public SortMode sortMode = SortMode.Largest; + public TargetRegion targetRegion = TargetRegion.Center; + public TargetOrientation targetOrientation = TargetOrientation.Landscape; + public boolean multiple = false; + public TargetGroup targetGroup = TargetGroup.Single; + public TargetIntersection targetIntersection = TargetIntersection.Up; + public DoubleCouple point = new DoubleCouple(); + public CalibrationMode calibrationMode = CalibrationMode.None; + public double dualTargetCalibrationM = 1; + public double dualTargetCalibrationB = 0; + + // 3d stuff + public MatOfPoint3f targetCornerMat = new MatOfPoint3f(); + public Number accuracy = 5; + private static MatOfPoint3f hexTargetMat = new MatOfPoint3f( + new Point3(-19.625, 0, 0), + new Point3(-9.819867, -17, 0), + new Point3(9.819867, -17, 0), + new Point3(19.625, 0, 0) + ); + + public StandardCVPipelineSettings() { + super(); + hexTargetMat.copyTo(targetCornerMat); + } + + + public boolean is3D = false; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java new file mode 100644 index 000000000..e99bcf434 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java @@ -0,0 +1,42 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; + +public class BlurPipe implements Pipe { + + private int blurSize; + + private Mat processBuffer = new Mat(); + private Mat outputMat = new Mat(); + + public BlurPipe(int blurSize) { + this.blurSize = blurSize; + } + + public void setConfig(int blurSize) { + this.blurSize = blurSize; + } + + @Override + public Pair run(Mat input) { + long processStartNanos = System.nanoTime(); + +// if (blurSize > 0) { +// input.copyTo(processBuffer); +// try { +// Imgproc.blur(processBuffer, processBuffer, new Size(blurSize, blurSize)); +// processBuffer.copyTo(outputMat); +// processBuffer.release(); +// } catch (CvException e) { +// System.err.println("(BlurPipe) Exception thrown by OpenCV: \n" + e.getMessage()); +// } +// } else { +// input.copyTo(outputMat); +// } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(input, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java new file mode 100644 index 000000000..74a8c6ab3 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java @@ -0,0 +1,128 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.enums.CalibrationMode; +import com.chameleonvision._2.vision.enums.TargetOrientation; +import com.chameleonvision._2.vision.enums.TargetRegion; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import org.apache.commons.lang3.tuple.Pair; +import org.apache.commons.math3.util.FastMath; +import org.opencv.core.Point; + +import java.util.ArrayList; +import java.util.List; + +public class Collect2dTargetsPipe implements Pipe, CaptureStaticProperties>, List> { + + + private CaptureStaticProperties camProps; + private CalibrationMode calibrationMode; + private DoubleCouple calibrationPoint; + private double calibrationM, calibrationB; + private TargetRegion targetRegion; + private TargetOrientation targetOrientation; + private List targets = new ArrayList<>(); + private Point[] vertices = new Point[4]; + + public Collect2dTargetsPipe(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) { + setConfig(calibrationMode, targetRegion, targetOrientation, calibrationPoint, calibrationM, calibrationB, camProps); + } + + public void setConfig(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) { + this.calibrationMode = calibrationMode; + this.calibrationPoint = calibrationPoint; + this.calibrationM = calibrationM; + this.calibrationB = calibrationB; + this.camProps = camProps; + this.targetRegion = targetRegion; + this.targetOrientation = targetOrientation; + } + + @Override + public Pair, Long> run(Pair, CaptureStaticProperties> inputPair) { + long processStartNanos = System.nanoTime(); + + targets.clear(); + var input = inputPair.getLeft(); + var imageArea = inputPair.getRight().imageArea; + + if (input.size() > 0) { + for (var t : input) { + t.minAreaRect.points(vertices); + Point bl = getMiddle(vertices[0], vertices[1]); + Point tl = getMiddle(vertices[1], vertices[2]); + Point tr = getMiddle(vertices[2], vertices[3]); + Point br = getMiddle(vertices[3], vertices[0]); + boolean orientation; + if (targetOrientation == TargetOrientation.Landscape) { + orientation = t.minAreaRect.size.width > t.minAreaRect.size.height; + } else { + orientation = t.minAreaRect.size.width < t.minAreaRect.size.height; + } + + Point result = t.minAreaRect.center; + switch (this.targetRegion) { + case Top: { + result = orientation ? tl : tr; + break; + } + case Bottom: { + result = orientation ? br : bl; + break; + } + case Left: { + result = orientation ? bl : tl; + break; + } + case Right: { + result = orientation ? tr : br; + break; + } + } + t.point = result; + + switch (this.calibrationMode) { + case Single: + if (this.calibrationPoint.equals(new DoubleCouple())) { + this.calibrationPoint.set(camProps.centerX, camProps.centerY); + } + t.calibratedX = this.calibrationPoint.getFirst(); + t.calibratedY = this.calibrationPoint.getSecond(); + break; + case None: + t.calibratedX = camProps.centerX; + t.calibratedY = camProps.centerY; + break; + case Dual: + t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM; + t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB; + break; + } + + t.pitch = calculatePitch(t.point.y, t.calibratedY); + t.yaw = calculateYaw(t.point.x, t.calibratedX); + t.area = t.minAreaRect.size.area() / imageArea; + + targets.add(t); + } + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(targets, processTime); + } + + private double calculatePitch(double pixelY, double centerY) { + double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / camProps.verticalFocalLength)); + return (pitch * -1); + } + + private double calculateYaw(double pixelX, double centerX) { + return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / camProps.horizontalFocalLength)); + } + + private Point getMiddle(Point p1, Point p2) { + return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dContoursPipe.java new file mode 100644 index 000000000..65bc872b6 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dContoursPipe.java @@ -0,0 +1,104 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision.common.util.ColorHelper; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Point; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; + +import java.awt.*; +import java.util.ArrayList; +import java.util.List; + +public class Draw2dContoursPipe implements Pipe>, Mat> { + + private final Draw2dContoursSettings settings; + private CaptureStaticProperties camProps; + + private Mat processBuffer = new Mat(); + private Mat outputMat = new Mat(); + + private Point[] vertices = new Point[4]; + private List drawnContours = new ArrayList<>(); + private MatOfPoint contour = new MatOfPoint(); + @SuppressWarnings("FieldCanBeLocal") + private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point(); + + + public Draw2dContoursPipe(Draw2dContoursSettings settings, CaptureStaticProperties camProps) { + this.settings = settings; + this.camProps = camProps; + } + + public void setConfig(boolean showMultiple,CaptureStaticProperties captureProps) { + settings.showMultiple = showMultiple; + camProps = captureProps; + } + + @Override + public Pair run(Pair> input) { + long processStartNanos = System.nanoTime(); + + if (settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) { +// input.getLeft().copyTo(processBuffer); +// processBuffer = input.getLeft(); + + if (input.getRight().size() > 0) { + for (int i = 0; i < input.getRight().size(); i++) { + if (i != 0 && !settings.showMultiple){ + break; + } + StandardCVPipeline.TrackedTarget target = input.getRight().get(i); + RotatedRect r = input.getRight().get(i).minAreaRect; + if (r == null) continue; + + drawnContours.forEach(Mat::release); + drawnContours.clear(); + drawnContours = new ArrayList<>(); + + r.points(vertices); + contour.fromArray(vertices); +// MatOfPoint contour = new MatOfPoint(vertices); + drawnContours.add(contour); + + + if (settings.showRotatedBox) { + Imgproc.drawContours(input.getLeft(), drawnContours, 0, ColorHelper.colorToScalar(settings.rotatedBoxColor), settings.boxOutlineSize); + } + + if (settings.showMaximumBox) { + Rect box = Imgproc.boundingRect(contour); + Imgproc.rectangle(input.getLeft(), new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), ColorHelper.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize); + } + if (settings.showCentroid) { + Imgproc.circle(input.getLeft(), target.point, 3, ColorHelper.colorToScalar(settings.centroidColor), 2); + + } + +// contour.release(); + } + } + +// processBuffer.copyTo(outputMat); +// processBuffer.release(); + } else { +// input.getLeft().copyTo(outputMat); + } + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(input.getLeft(), processTime); + } + + public static class Draw2dContoursSettings { + public boolean showCentroid = false; + public boolean showMultiple = false; + public int boxOutlineSize = 0; + public boolean showRotatedBox = false; + public boolean showMaximumBox = false; + public Color centroidColor = Color.GREEN; + public Color rotatedBoxColor = Color.BLUE; + public Color maximumBoxColor = Color.RED; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dCrosshairPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dCrosshairPipe.java new file mode 100644 index 000000000..9127c1804 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Draw2dCrosshairPipe.java @@ -0,0 +1,85 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.enums.CalibrationMode; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision.common.util.ColorHelper; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; + +import java.awt.*; +import java.util.List; + +public class Draw2dCrosshairPipe implements Pipe>, Mat> { + + //Settings + private Draw2dCrosshairPipeSettings crosshairSettings; + private CalibrationMode calibrationMode; + private DoubleCouple calibrationPoint; + private double calibrationM, calibrationB; + + + private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point(); + + public Draw2dCrosshairPipe(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) { + setConfig(crosshairSettings, calibrationMode, calibrationPoint, calibrationM, calibrationB); + } + + public void setConfig(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) { + this.crosshairSettings = crosshairSettings; + this.calibrationMode = calibrationMode; + this.calibrationPoint = calibrationPoint; + this.calibrationM = calibrationM; + this.calibrationB = calibrationB; + } + + @Override + public Pair run(Pair> inputPair) { + long processStartNanos = System.nanoTime(); + Mat image = inputPair.getLeft(); +// List targets = inputPair.getRight(); + double x, y; + double scale = image.cols() / 32.0; + + drawCrosshair: + if (this.crosshairSettings.showCrosshair) { + x = image.cols() / 2.0; + y = image.rows() / 2.0; + switch (this.calibrationMode) { + case Single: + if(this.calibrationPoint.equals(new DoubleCouple())) + { + this.calibrationPoint.set(x, y); + } + x = this.calibrationPoint.getFirst().intValue(); + y = this.calibrationPoint.getSecond().intValue(); + break; + case Dual: +// if (targets != null && !targets.isEmpty()) { +// x = targets.get(0).calibratedX; +// y = targets.get(0).calibratedY; +// //TODO dual point calibration crosshair checks +// } else +// break drawCrosshair; + break; + } + xMax.set(new double[]{x + scale, y}); + xMin.set(new double[]{x - scale, y}); + yMax.set(new double[]{x, y + scale}); + yMin.set(new double[]{x, y - scale}); + Imgproc.line(inputPair.getLeft(), xMax, xMin, ColorHelper.colorToScalar(this.crosshairSettings.crosshairColor), 2); + Imgproc.line(inputPair.getLeft(), yMax, yMin, ColorHelper.colorToScalar(this.crosshairSettings.crosshairColor), 2); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(inputPair.getLeft(), processTime); + } + + public static class Draw2dCrosshairPipeSettings { + public boolean showCrosshair = true; + public Color crosshairColor = Color.GREEN; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/DrawSolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/DrawSolvePNPPipe.java new file mode 100644 index 000000000..eab41c638 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/DrawSolvePNPPipe.java @@ -0,0 +1,126 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.chameleonvision.common.util.ColorHelper; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; + +import java.awt.*; +import java.util.List; +import java.util.ArrayList; +import java.util.stream.Collectors; + +public class DrawSolvePNPPipe implements Pipe>, Mat> { + + private MatOfPoint3f boxCornerMat = new MatOfPoint3f(); + + public Scalar green = ColorHelper.colorToScalar(Color.GREEN); + public Scalar blue = ColorHelper.colorToScalar(Color.BLUE); + public Scalar red = ColorHelper.colorToScalar(Color.RED); + public Scalar orange = ColorHelper.colorToScalar(Color.orange); + + public DrawSolvePNPPipe(StandardCVPipelineSettings standardCVPipelineSettings, CameraCalibrationConfig settings) { + setConfig(settings); + setBox(standardCVPipelineSettings.targetCornerMat); + } + + + private void setBox(MatOfPoint3f mat) { + boxCornerMat.release(); + var list = mat.toList(); + var auxList = list.stream().map(it -> new Point3(it.x, it.y, it.z + 6)).collect(Collectors.toList()); + var finalList = new ArrayList<>(list); + finalList.addAll(auxList); + boxCornerMat.fromList(finalList); + } + + + public void setConfig(StandardCVPipelineSettings settings) { + setBox(settings.targetCornerMat); + } + + private Mat cameraMatrix = new Mat(); + private MatOfDouble distortionCoefficients = new MatOfDouble(); + + public void setConfig(CameraCalibrationConfig config) { + if (config == null) { + System.err.println("got passed a null config! Returning..."); + return; + } + setConfig(config.getCameraMatrixAsMat(), config.getDistortionCoeffsAsMat()); + } + + public void setConfig(Mat cameraMatrix_, MatOfDouble distortionMatrix_) { + this.cameraMatrix = cameraMatrix_; + this.distortionCoefficients = distortionMatrix_; + } + + MatOfPoint2f imagePoints = new MatOfPoint2f(); + + @Override + public Pair run(Pair> targets) { + long processStartNanos = System.nanoTime(); + + var image = targets.getLeft(); + for (var it : targets.getRight()) { + + try { + Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat(), 0); + } catch (Exception e) { + e.printStackTrace(); + } + var pts = imagePoints.toList(); + + // draw left and right targets if possible + if (it.leftRightDualTargetPair != null) { + var left = it.leftRightDualTargetPair.getLeft(); + var right = it.leftRightDualTargetPair.getRight(); + Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4); + Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2); + } + + // draw poly dp + var list = it.approxPoly.toList(); + for (int i = 0; i < list.size(); i++) { + var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1); + Imgproc.line(image, list.get(i), next, red, 2); + } + + // draw center + Imgproc.circle(image, it.minAreaRect.center, 5, red); + + // draw corners + for (int i = 0; i < it.imageCornerPoints.rows(); i++) { + var point = new Point(it.imageCornerPoints.get(i, 0)); + Imgproc.circle(image, point, 4, green, 5); + } + + // sketch out floor + Imgproc.line(image, pts.get(0), pts.get(1), green, 3); + Imgproc.line(image, pts.get(1), pts.get(2), green, 3); + Imgproc.line(image, pts.get(2), pts.get(3), green, 3); + Imgproc.line(image, pts.get(3), pts.get(0), green, 3); + + // draw pillars + Imgproc.line(image, pts.get(0), pts.get(4), blue, 3); + Imgproc.line(image, pts.get(1), pts.get(5), blue, 3); + Imgproc.line(image, pts.get(2), pts.get(6), blue, 3); + Imgproc.line(image, pts.get(3), pts.get(7), blue, 3); + + // draw top + Imgproc.line(image, pts.get(4), pts.get(5), red, 3); + Imgproc.line(image, pts.get(5), pts.get(6), red, 3); + Imgproc.line(image, pts.get(6), pts.get(7), red, 3); + Imgproc.line(image, pts.get(7), pts.get(4), red, 3); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(image, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/ErodeDilatePipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/ErodeDilatePipe.java new file mode 100644 index 000000000..af41d9584 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/ErodeDilatePipe.java @@ -0,0 +1,51 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +public class ErodeDilatePipe implements Pipe { + + private boolean erode; + private boolean dilate; + private Mat kernel; + + public ErodeDilatePipe(boolean erode, boolean dilate, int kernelSize) { + this.erode = erode; + this.dilate = dilate; + kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize)); + } + + public void setConfig(boolean erode, boolean dilate, int kernelSize) { + this.erode = erode; + this.dilate = dilate; + kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize)); + } + + @Override + public Pair run(Mat input) { + long processStartNanos = System.nanoTime(); + + if (erode || dilate) { +// input.copyTo(processBuffer); + + if (erode) { + Imgproc.erode(input, input, kernel); + } + + if (dilate) { + Imgproc.dilate(input, input, kernel); + } + +// processBuffer.copyTo(outputMat); +// processBuffer.release(); + } else { +// input.copyTo(outputMat); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(input, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java new file mode 100644 index 000000000..6536e500a --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java @@ -0,0 +1,77 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Rect; +import org.opencv.imgproc.Imgproc; + +import java.util.ArrayList; +import java.util.List; + +public class FilterContoursPipe implements Pipe, List> { + + private DoubleCouple area; + private DoubleCouple ratio; + private DoubleCouple extent; + private CaptureStaticProperties camProps; + + private List filteredContours = new ArrayList<>(); + + public FilterContoursPipe(DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, CaptureStaticProperties camProps) { + this.area = area; + this.ratio = ratio; + this.extent = extent; + this.camProps = camProps; + } + + public void setConfig(DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, CaptureStaticProperties camProps) { + this.area = area; + this.ratio = ratio; + this.extent = extent; + this.camProps = camProps; + } + + @Override + public Pair, Long> run(List input) { + long processStartNanos = System.nanoTime(); + + filteredContours.clear(); + + if (input.size() > 0) { + for (MatOfPoint Contour : input) { + try { + double contourArea = Imgproc.contourArea(Contour); + double AreaRatio = (contourArea / camProps.imageArea) * 100; + double minArea = (MathUtils.sigmoid(area.getFirst())); + double maxArea = (MathUtils.sigmoid(area.getFirst())); + if (AreaRatio < minArea || AreaRatio > maxArea) { + continue; + } + var rect = Imgproc.minAreaRect(new MatOfPoint2f(Contour.toArray())); + double minExtent = (extent.getFirst() * rect.size.area()) / 100; + double maxExtent = (extent.getSecond() * rect.size.area()) / 100; + if (contourArea <= minExtent || contourArea >= maxExtent) { + continue; + } + Rect bb = Imgproc.boundingRect(Contour); + double aspectRatio = ((double)bb.width / bb.height); + if (aspectRatio < ratio.getFirst() || aspectRatio > ratio.getSecond()) { + continue; + } + filteredContours.add(Contour); + } catch (Exception e) { + System.err.println("Error while filtering contours"); + e.printStackTrace(); + } + } + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(filteredContours, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java new file mode 100644 index 000000000..16d4face7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java @@ -0,0 +1,29 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.imgproc.Imgproc; + +import java.util.ArrayList; +import java.util.List; + +public class FindContoursPipe implements Pipe> { + + private List foundContours = new ArrayList<>(); + + public FindContoursPipe() {} + + @Override + public Pair, Long> run(Mat input) { + long processStartNanos = System.nanoTime(); + + foundContours.clear(); + + Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(foundContours, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java new file mode 100644 index 000000000..981186758 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java @@ -0,0 +1,195 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.enums.TargetGroup; +import com.chameleonvision._2.vision.enums.TargetIntersection; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision.common.util.math.MathUtils; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; + +public class GroupContoursPipe implements Pipe, List> { + + private static final Comparator sortByMomentsX = + Comparator.comparingDouble(GroupContoursPipe::calcMomentsX); + + private TargetGroup group; + private TargetIntersection intersection; + + private MatOfPoint2f contourBuffer = new MatOfPoint2f(); + + private List groupedContours = new ArrayList<>(); + private MatOfPoint2f intersectMatA = new MatOfPoint2f(); + private MatOfPoint2f intersectMatB = new MatOfPoint2f(); + + public GroupContoursPipe(TargetGroup group, TargetIntersection intersection) { + this.group = group; + this.intersection = intersection; + } + + public void setConfig(TargetGroup group, TargetIntersection intersection) { + this.group = group; + this.intersection = intersection; + } + + @Override + public Pair, Long> run(List input) { + long processStartNanos = System.nanoTime(); + + groupedContours.forEach(StandardCVPipeline.TrackedTarget::release); + groupedContours.clear(); + contourBuffer.release(); + + if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) { + + List sorted = new ArrayList<>(input); + sorted.sort(sortByMomentsX); + + Collections.reverse(sorted); + + switch (group) { + case Single: { + input.forEach(c -> { + contourBuffer.fromArray(c.toArray()); + if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contourBuffer); + Rect boundingRect = Imgproc.boundingRect(contourBuffer); + var target = new StandardCVPipeline.TrackedTarget(); + target.minAreaRect = rect; + target.rawContour = contourBuffer; + target.boundingRect = boundingRect; + groupedContours.add(target); + } + }); + break; + } + case Dual: { + for (var i = 0; i < input.size(); i++) { + List finalContourList = new ArrayList<>(input.get(i).toList()); + + try { + MatOfPoint firstContour = input.get(i); + MatOfPoint secondContour = input.get(i + 1); + + if (isIntersecting(firstContour, secondContour)) { + finalContourList.addAll(secondContour.toList()); + } else { + finalContourList.clear(); + continue; + } + + intersectMatA.release(); + intersectMatB.release(); + + contourBuffer.fromList(finalContourList); + + if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contourBuffer); + Rect boundingRect = Imgproc.boundingRect(contourBuffer); + var target = new StandardCVPipeline.TrackedTarget(); + target.minAreaRect = rect; + target.boundingRect = boundingRect; + // find left and right bouding rectangles + target.leftRightDualTargetPair = + Pair.of(Imgproc.boundingRect(firstContour), + Imgproc.boundingRect(secondContour)); + + // find left and right min area rectangles + tempRectMat.fromArray(firstContour.toArray()); + var minAreaRect1 = Imgproc.minAreaRect(tempRectMat); + tempRectMat.fromArray(secondContour.toArray()); + var minAreaRect2 = Imgproc.minAreaRect(tempRectMat); + target.leftRightRotatedRect = + Pair.of(minAreaRect1, minAreaRect2); + + target.rawContour = contourBuffer; + + groupedContours.add(target); + + firstContour.release(); + secondContour.release(); + + // skip the next contour because it's been grouped already + i += 1; + } + } catch (IndexOutOfBoundsException e) { + finalContourList.clear(); + } + } + break; + } + } + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(groupedContours, processTime); + } + + MatOfPoint2f tempRectMat = new MatOfPoint2f(); + + private static double calcMomentsX(MatOfPoint c) { + Moments m = Imgproc.moments(c); + return (m.get_m10() / m.get_m00()); + } + + private boolean isIntersecting(MatOfPoint contourOne, MatOfPoint contourTwo) { + if (intersection.equals(TargetIntersection.None)) { + return true; + } + + try { + intersectMatA.fromArray(contourOne.toArray()); + intersectMatB.fromArray(contourTwo.toArray()); + RotatedRect a = Imgproc.fitEllipse(intersectMatA); + RotatedRect b = Imgproc.fitEllipse(intersectMatB); + double mA = MathUtils.toSlope(a.angle); + double mB = MathUtils.toSlope(b.angle); + double x0A = a.center.x; + double y0A = a.center.y; + double x0B = b.center.x; + double y0B = b.center.y; + double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB); + double intersectionY = (mA * (intersectionX - x0A)) + y0A; + double massX = (x0A + x0B) / 2; + double massY = (y0A + y0B) / 2; + switch (intersection) { + case Up: { + if (intersectionY < massY) { + return true; + } + break; + } + case Down: { + if (intersectionY > massY) { + return true; + } + + break; + } + case Left: { + if (intersectionX < massX) { + + return true; + } + break; + } + case Right: { + if (intersectionX > massX) { + return true; + } + break; + } + } + return false; + } catch (Exception e) { + return false; + } + } +} \ No newline at end of file diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java new file mode 100644 index 000000000..be879d236 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java @@ -0,0 +1,46 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Core; +import org.opencv.core.CvException; +import org.opencv.core.Mat; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +public class HsvPipe implements Pipe { + + private Scalar hsvLower; + private Scalar hsvUpper; + + private Mat processBuffer = new Mat(); + private Mat outputMat = new Mat(); + + public HsvPipe(Scalar hsvLower, Scalar hsvUpper) { + this.hsvLower = hsvLower; + this.hsvUpper = hsvUpper; + } + + public void setConfig(Scalar hsvLower, Scalar hsvUpper) { + this.hsvLower = hsvLower; + this.hsvUpper = hsvUpper; + } + + @Override + public Pair run(Mat input) { + long processStartNanos = System.nanoTime(); + + input.copyTo(outputMat); + + try { + Imgproc.cvtColor(outputMat, outputMat, Imgproc.COLOR_BGR2HSV, 3); + Core.inRange(outputMat, hsvLower, hsvUpper, outputMat); + } catch (CvException e) { + System.err.println("(HsvPipe) Exception thrown by OpenCV: \n" + e.getMessage()); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(outputMat, processTime); + } +} + diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java new file mode 100644 index 000000000..63113e7f2 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java @@ -0,0 +1,50 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.CvException; +import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; + +public class OutputMatPipe implements Pipe, Mat> { + + private boolean showThresholded; + + private Mat processBuffer = new Mat(); + private Mat outputMat = new Mat(); + + public OutputMatPipe(boolean showThresholded) { + this.showThresholded = showThresholded; + } + + public void setConfig(boolean showThresholded) { + this.showThresholded = showThresholded; + } + + /** + * + * @param input Input object for pipe + * Left is raw camera mat (8UC3), Right is HSV threshold mat (8UC1) + * @return Returns desired output Mat, and processing time in nanoseconds + */ + @Override + public Pair run(Pair input) { + long processStartNanos = System.nanoTime(); + + if (showThresholded) { + try { + input.getRight().copyTo(processBuffer); + Imgproc.cvtColor(processBuffer, processBuffer, Imgproc.COLOR_GRAY2BGR, 3); + processBuffer.copyTo(outputMat); + processBuffer.release(); + } catch (CvException e) { + System.err.println("(OutputMat) Exception thrown by OpenCV: \n" + e.getMessage()); + } + } else { + input.getLeft().copyTo(outputMat); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(outputMat, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java new file mode 100644 index 000000000..cc7c27585 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java @@ -0,0 +1,55 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.enums.ImageFlipMode; +import com.chameleonvision._2.vision.enums.ImageRotationMode; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Core; +import org.opencv.core.Mat; + +public class RotateFlipPipe implements Pipe { + + private ImageRotationMode rotation; + private ImageFlipMode flip; + + private Mat processBuffer = new Mat(); + private Mat outputMat = new Mat(); + + public RotateFlipPipe(ImageRotationMode rotation, ImageFlipMode flip) { + this.rotation = rotation; + this.flip = flip; + } + + public void setConfig(ImageRotationMode rotation, ImageFlipMode flip) { + this.rotation = rotation; + this.flip = flip; + } + + @Override + public Pair run(Mat input) { + long processStartNanos = System.nanoTime(); + + boolean shouldFlip = !flip.equals(ImageFlipMode.NONE); + boolean shouldRotate = !rotation.equals(ImageRotationMode.DEG_0); + + if (shouldFlip || shouldRotate) { +// input.copyTo(processBuffer); + + if (shouldFlip) { + Core.flip(input, input, flip.value); + } + + if (shouldRotate) { + Core.rotate(input, input, rotation.value); + } + +// processBuffer.copyTo(outputMat); +// processBuffer.release(); + } else { +// input.copyTo(outputMat); + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(input, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java new file mode 100644 index 000000000..e1cfcd3e1 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java @@ -0,0 +1,443 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import org.apache.commons.lang3.tuple.Pair; +import org.apache.commons.math3.util.FastMath; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; + +import java.util.*; +import java.util.stream.Collectors; + +public class SolvePNPPipe implements Pipe, Mat>, List> { + + private Double tilt_angle; + private MatOfPoint3f objPointsMat = new MatOfPoint3f(); + private Mat rVec = new Mat(); + private Mat tVec = new Mat(); + private Mat rodriguez = new Mat(); + private Mat pzero_world = new Mat(); + private Mat cameraMatrix = new Mat(); + Mat rot_inv = new Mat(); + Mat kMat = new Mat(); + private MatOfDouble distortionCoefficients = new MatOfDouble(); + private List poseList = new ArrayList<>(); + Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); + Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); + private double distanceDivisor = 1.0; + Mat scaledTvec = new Mat(); + MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f(); + MatOfPoint2f polyOutput = new MatOfPoint2f(); + private Mat greyImg = new Mat(); + private double accuracyPercentage = 0.2; + + public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) { + super(); + setCameraCoeffs(calibration); +// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); + // TODO add proper year differentiation + set2020Target(true); + + this.tilt_angle = tilt.getRadians(); + } + + public void set2020Target(boolean isHighGoal) { + if(isHighGoal) { + // tl, bl, br, tr is the order + List corners = List.of( + + new Point3(-19.625, 0, 0), + new Point3(-9.819867, -17, 0), + new Point3(9.819867, -17, 0), + new Point3(19.625, 0, 0)); + setObjectCorners(corners); + } else { + setBoundingBoxTarget(7, 11); + } + } + + public void setBoundingBoxTarget(double targetWidth, double targetHeight) { + // order is left top, left bottom, right bottom, right top + + List corners = List.of( + new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0), + new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0) + ); + setObjectCorners(corners); + } + + public void setObjectCorners(List objectCorners) { + objPointsMat.release(); + objPointsMat = new MatOfPoint3f(); + objPointsMat.fromList(objectCorners); + } + + public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) { + setCameraCoeffs(camConfig); +// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); + // TODO add proper year differentiation + tilt_angle = tilt.getRadians(); + this.objPointsMat = settings.targetCornerMat; + this.accuracyPercentage = settings.accuracy.doubleValue(); + } + + private void setCameraCoeffs(CameraCalibrationConfig settings) { + if(settings == null) { + System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not! Please calibrate to use solvePNP."); + return; + } + if(cameraMatrix != settings.getCameraMatrixAsMat()) { + cameraMatrix.release(); + settings.getCameraMatrixAsMat().copyTo(cameraMatrix); + } + if(distortionCoefficients != settings.getDistortionCoeffsAsMat()) { + distortionCoefficients.release(); + settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); + } + this.distanceDivisor = settings.squareSize; + } + + @Override + public Pair, Long> run(Pair, Mat> imageTargetPair) { + long processStartNanos = System.nanoTime(); + var targets = imageTargetPair.getLeft(); + var image = imageTargetPair.getRight(); + Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY); + poseList.clear(); + for(var target: targets) { + MatOfPoint2f corners; + if(target.leftRightRotatedRect == null) { + corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target); + } else { + corners = findCorner2019(target); + } +// var corners = findCorner2019(target); + if(corners == null) continue; + +// // use best features to track +// corners = refineCornersByBestTrack(corners, greyImg, target); + + // refine the estimate +// corners = refineCornerEstimateSubPix(corners, greyImg); + + var pose = calculatePose(corners, target); + if(pose != null) poseList.add(pose); + } + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(poseList, processTime); + } + + private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) { + if(target.leftRightDualTargetPair == null) return null; + + var left = target.leftRightDualTargetPair.getLeft(); + var right = target.leftRightDualTargetPair.getRight(); + + // flip if the "left" target is to the right + if(left.x > right.x) { + var temp = left; + left = right; + right = temp; + } + + var points = new MatOfPoint2f(); + points.fromArray( + new Point(left.x, left.y + left.height), + new Point(left.x, left.y), + new Point(right.x + right.width, right.y), + new Point(right.x + right.width, right.y + right.height) + ); + return points; + } + + MatOfPoint2f target2020ResultMat = new MatOfPoint2f(); + + private double distanceBetween(Point a, Point b) { + return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2)); + } + + /** + * Find the target using the outermost tape corners and a 2020 target. + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, double accuracyPercentage) { + if(target.rawContour.cols() < 1) return null; + + var centroid = target.minAreaRect.center; + Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); + + // algorithm from team 4915 + + // Contour perimeter + var peri = Imgproc.arcLength(target.rawContour, true); + // approximating a shape around the contours + // Can be tuned to allow/disallow hulls + // Approx is the number of vertices + // Ramer–Douglas–Peucker algorithm + // we want a number between 0 and 0.16 out of a percentage from 0 to 100 + // so take accuracy and divide by 600 + Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true); + + var area = Imgproc.moments(polyOutput); + +// if (area.get_m00() < 200) { +// return null; +// } + + var polyList = polyOutput.toList(); + + polyOutput.copyTo(target.approxPoly); + + // left top, left bottom, right bottom, right top + var boundingBoxCorners = findBoundingBoxCorners(target).toList(); + + try { + + // top left and top right are the poly corners closest to the bouding box tl and tr + var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); + var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); + + var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + +// polyList = new ArrayList<>(polyList); +// polyList.removeAll(List.of(tl, tr, bl, br)); +// +// var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); +// var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); +// +// var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); +// var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + + target2020ResultMat.release(); + target2020ResultMat.fromList(List.of(tl, bl, br, tr));//, tr2, br2, bl2, tl2)); + + return target2020ResultMat; + } catch (NoSuchElementException e) { + return null; + } + } + + /** + * Find the target using the outermost tape corners and a dual target. + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { + if(target.leftRightRotatedRect == null) return null; + + var centroid = target.minAreaRect.center; + Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); + + var left = target.leftRightRotatedRect.getLeft(); + var right = target.leftRightRotatedRect.getRight(); + + // flip if the "left" target is to the right + if(left.center.x > right.center.x) { + var temp = left; + left = right; + right = temp; + } + + var leftPoints = new Point[4]; + left.points(leftPoints); + var rightPoints = new Point[4]; + right.points(rightPoints); + ArrayList combinedList = new ArrayList<>(List.of(leftPoints)); + combinedList.addAll(List.of(rightPoints)); + + // start looking in the top left quadrant + var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + + return boundingBoxResultMat; + } + + /** + * + * @param target the target to find the corners of. + * @return the corners. left top, left bottom, right bottom, right top + */ + private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) { + +// List> list = new ArrayList<>(); +// // find the corners based on the bounding box +// // order is left top, left bottom, right bottom, right top + + // extract the corners + var points = new Point[4]; + target.minAreaRect.points(points); + + // find the tl/tr/bl/br corners + // first, min by left/right + var list_ = Arrays.asList(points); + list_.sort(leftRightComparator); + // of this, we now have left and right + // sort to get top and bottom + var left = new ArrayList<>(List.of(list_.get(0), list_.get(1))); + left.sort(verticalComparator); + var right = new ArrayList<>(List.of(list_.get(2), list_.get(3))); + right.sort(verticalComparator); + + // tl tr bl br + var tl = left.get(0); + var bl = left.get(1); + var tr = right.get(0); + var br = right.get(1); + + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + + return boundingBoxResultMat; + } + + MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f(); + + private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) { + + MatOfPoint approxf1 = new MatOfPoint(); + var origCornerList = new ArrayList<>(corners.toList()); + approxf1.fromList(origCornerList.stream() + .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y)) + .collect(Collectors.toList()) + ); + var croppedImage = greyImg.submat(target.boundingRect); + + Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5); + + // at this point corners is still unmodified so let's map it + List tempList = new ArrayList<>(); + + // shift all points back into global pose + var reshiftedList = approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y)) + .collect(Collectors.toList()); + for(Point p: origCornerList) { + // find the goodFeaturesToTrack corner closest to me + var closestPoint = reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p))); + if(closestPoint.isEmpty()) { + tempList.add(p); + reshiftedList.remove(p); + } else { + tempList.add(closestPoint.get()); + reshiftedList.remove(closestPoint.get()); + } + } + + goodFeatureToTrackRetval.fromList(tempList); + return goodFeatureToTrackRetval; + } + + // Set the needed parameters to find the refined corners + Size winSize = new Size(4, 4); + Size zeroZone = new Size(-1, -1); // we don't need a zero zone + TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001); + + private boolean shouldRefineCorners = true; + + /** + * Refine an estimated corner position using the cornerSubPixel algorithm. + * + * TODO should this be here or before the points are chosen? + * + * @param corners the corners detected -- this mat is modified! + * @param greyImg the image taken by the camera as color + * @return the updated mat, same as the corner mat passed in. + */ + private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) { + if(!shouldRefineCorners) return corners; // just return + Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria); + + return corners; + } + +// NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("tvec"); +// NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("rvec"); + + public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) { + if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) { + System.err.println("can't do solvePNP with invalid params!"); + return null; + } + + imageCornerPoints.copyTo(target.imageCornerPoints); + + try { + Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec); + } catch (Exception e) { + e.printStackTrace(); + return null; + } + +// tvecE.setString(tVec.dump()); +// rvecE.setString(rVec.dump()); + + // Algorithm from team 5190 Green Hope Falcons + +// var tilt_angle = 0.0; // TODO add to settings + + // the left/right distance to the target, unchanged by tilt + var x = tVec.get(0, 0)[0]; + + // Z distance in the flat plane is given by + // Z_field = z cos theta + y sin theta + var z = tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle); + + // find skew of the target relative to the camera + // from ligerbots: + // rot, _ = cv2.Rodrigues(rvec) + // rot_inv = rot.transpose() + // pzero_world = numpy.matmul(rot_inv, -tvec) + // angle2 = math.atan2(pzero_world[0][0], pzero_world[2][0] + + Calib3d.Rodrigues(rVec, rodriguez); + Core.transpose(rodriguez, rot_inv); // rodrigurz.t() + + scaledTvec = matScale(tVec, -1); + Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world); + + var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]); + + var targetRotation = -angle2; // radians + + // We want a vector that is X forward and Y left. + // We have a Z_field (out of the camera projected onto the field), and an X left/right. + // so Z_field becomes X, and X becomes Y + + //noinspection SuspiciousNameCombination + var targetLocation = new Translation2d(z, -x).times(25.4 / 1000d / distanceDivisor); + target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation)); + target.rVector = rVec; + target.tVector = tVec; + + return target; + } + + /** + * Element-wise scale a matrix by a given factor + * @param src the source matrix + * @param factor by how much to scale each element + * @return the scaled matrix + */ + public Mat matScale(Mat src, double factor) { + Mat dst = new Mat(src.rows(),src.cols(),src.type()); + Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this + Core.multiply(src, s, dst); + return dst; + } + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java new file mode 100644 index 000000000..c67f5fca4 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java @@ -0,0 +1,93 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.enums.SortMode; +import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import org.apache.commons.lang3.tuple.Pair; +import org.apache.commons.math3.util.FastMath; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; + +public class SortContoursPipe implements Pipe, List> { + + private final Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance); + + private static final Comparator SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area()); + private static final Comparator SortBySmallestComparator = SortByLargestComparator.reversed(); + + private static final Comparator SortByHighestComparator = (rect1, rect2) -> Double.compare(rect1.minAreaRect.center.y, rect2.minAreaRect.center.y); + private static final Comparator SortByLowestComparator = SortByHighestComparator.reversed(); + + public static final Comparator SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x); + private static final Comparator SortByRightmostComparator = SortByLeftmostComparator.reversed(); + + private SortMode sort; + private CaptureStaticProperties camProps; + private int maxTargets; + + private List sortedContours = new ArrayList<>(); + + public SortContoursPipe(SortMode sort, CaptureStaticProperties camProps, int maxTargets) { + this.sort = sort; + this.camProps = camProps; + this.maxTargets = maxTargets; + } + + public void setConfig(SortMode sort, CaptureStaticProperties camProps, int maxTargets) { + this.sort = sort; + this.camProps = camProps; + this.maxTargets = maxTargets; + } + + @Override + public Pair, Long> run(List input) { + long processStartNanos = System.nanoTime(); + + sortedContours.clear(); + + if (input.size() > 0) { + sortedContours.addAll(input); + + switch (sort) { + case Largest: + sortedContours.sort(SortByLargestComparator); + break; + case Smallest: + sortedContours.sort(SortBySmallestComparator); + break; + case Highest: + sortedContours.sort(SortByHighestComparator); + break; + case Lowest: + sortedContours.sort(SortByLowestComparator); + break; + case Leftmost: + sortedContours.sort(SortByLeftmostComparator); + break; + case Rightmost: + sortedContours.sort(SortByRightmostComparator); + break; + case Centermost: + sortedContours.sort(SortByCentermostComparator); + break; + default: + break; + } + } + + var sublistedContors = new ArrayList<>(sortedContours.subList(0, Math.min(input.size(), maxTargets - 1))); + sortedContours.subList(Math.min(input.size(), maxTargets - 1), sortedContours.size()).forEach(StandardCVPipeline.TrackedTarget::release); + sortedContours.clear(); + sortedContours = new ArrayList<>(); + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(sublistedContors, processTime); + } + + private double calcSquareCenterDistance(StandardCVPipeline.TrackedTarget rect) { + return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.minAreaRect.center.x, 2) + FastMath.pow(camProps.centerY - rect.minAreaRect.center.y, 2)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SpeckleRejectPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SpeckleRejectPipe.java new file mode 100644 index 000000000..b4240f509 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SpeckleRejectPipe.java @@ -0,0 +1,54 @@ +package com.chameleonvision._2.vision.pipeline.pipes; + +import com.chameleonvision._2.vision.pipeline.Pipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.MatOfPoint; +import org.opencv.imgproc.Imgproc; + +import java.util.ArrayList; +import java.util.List; + +public class SpeckleRejectPipe implements Pipe, List> { + + private double minPercentOfAvg; + + private List despeckledContours = new ArrayList<>(); + + public SpeckleRejectPipe(double minPercentOfAvg) { + this.minPercentOfAvg = minPercentOfAvg; + } + + public void setConfig(double minPercentOfAvg) { + this.minPercentOfAvg = minPercentOfAvg; + } + + @Override + public Pair, Long> run(List input) { + long processStartNanos = System.nanoTime(); + + despeckledContours.forEach(MatOfPoint::release); + despeckledContours.clear(); + despeckledContours = new ArrayList<>(); + + if (input.size() > 0) { + double averageArea = 0.0; + + for (MatOfPoint c : input) { + averageArea += Imgproc.contourArea(c); + } + + averageArea /= input.size(); + + double minAllowedArea = minPercentOfAvg / 100.0 * averageArea; + + for (MatOfPoint c : input) { + if (Imgproc.contourArea(c) >= minAllowedArea) { + despeckledContours.add(c); + } + } + } + + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(despeckledContours, processTime); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java new file mode 100644 index 000000000..80a5f911d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java @@ -0,0 +1,251 @@ +package com.chameleonvision._2.web; + +import com.chameleonvision._2.Main; +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.network.NetworkManager; +import com.chameleonvision._2.util.Helpers; +import com.chameleonvision._2.util.ProgramDirectoryUtilities; +import com.chameleonvision._2.vision.VisionManager; +import com.chameleonvision._2.vision.VisionProcess; +import com.chameleonvision._2.vision.camera.USBCameraCapture; +import com.chameleonvision._2.vision.pipeline.PipelineManager; +import com.chameleonvision._2.vision.pipeline.impl.Calibrate3dPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; +import com.chameleonvision.common.datatransfer.networktables.NetworkTablesManager; +import com.chameleonvision.common.networking.NetworkMode; +import com.chameleonvision.common.util.Platform; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import io.javalin.http.Context; +import io.javalin.http.UploadedFile; +import org.opencv.core.Point3; + +import java.io.File; +import java.io.FileOutputStream; +import java.io.OutputStream; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +public class RequestHandler { + + private static final ObjectMapper kObjectMapper = new ObjectMapper(); + + public static void onGeneralSettings(Context ctx) { + ObjectMapper objectMapper = kObjectMapper; + try { + Map map = objectMapper.readValue(ctx.body(), Map.class); + + // TODO: change to function, to restart NetworkTables + int newTeamNumber = (int) map.get("teamNumber"); + if (newTeamNumber != ConfigManager.settings.teamNumber && !NetworkTablesManager.isServer) { + NetworkTablesManager.setTeamClientMode(); + } + ConfigManager.settings.teamNumber = newTeamNumber; + + ConfigManager.settings.connectionType = NetworkMode.values()[(int) map.get("connectionType")]; + ConfigManager.settings.ip = (String) map.get("ip"); + ConfigManager.settings.netmask = (String) map.get("netmask"); + ConfigManager.settings.gateway = (String) map.get("gateway"); + ConfigManager.settings.hostname = (String) map.get("hostname"); + ConfigManager.saveGeneralSettings(); + // setting up network config after saving + boolean isStatic = ConfigManager.settings.connectionType.equals(NetworkMode.STATIC); + + boolean state = NetworkManager.setHostname(ConfigManager.settings.hostname) && NetworkManager.setNetwork(isStatic, ConfigManager.settings.ip, ConfigManager.settings.netmask, ConfigManager.settings.gateway); + if (state) { + ctx.status(200); + } else { + ctx.result("Something went wrong while setting network configuration"); + ctx.status(501); + } + SocketHandler.sendFullSettings(); + } catch (JsonProcessingException e) { + ctx.status(500); + } + } + + public static void onDuplicatePipeline(Context ctx) { + ObjectMapper objectMapper = kObjectMapper; + try { + Map data = objectMapper.readValue(ctx.body(), Map.class); + + int cameraIndex = (Integer) data.getOrDefault("camera", -1); + + var pipelineIndex = (Integer) data.get("pipeline"); + StandardCVPipelineSettings origPipeline = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getPipeline(pipelineIndex).settings; + String tmp = objectMapper.writeValueAsString(origPipeline); + StandardCVPipelineSettings newPipeline = objectMapper.readValue(tmp, StandardCVPipelineSettings.class); + + if (cameraIndex == -1) { // same camera + + VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline); + + } else { // another camera + var cam = VisionManager.getVisionProcessByIndex(cameraIndex); + if (cam != null) { + if (cam.getCamera().getProperties().videoModes.size() < newPipeline.videoModeIndex) { + newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1; + } + if (newPipeline.is3D) { + var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex)); + if (calibration == null) { + newPipeline.is3D = false; + } + } + VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline, cam); + ctx.status(200); + } else { + ctx.status(500); + } + } + } catch (JsonProcessingException ex) { + ctx.status(500); + } + } + + + public static void onCameraSettings(Context ctx) { + ObjectMapper objectMapper = kObjectMapper; + try { + Map camSettings = objectMapper.readValue(ctx.body(), Map.class); + + VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess(); + USBCameraCapture currentCamera = currentVisionProcess.getCamera(); + + double newFOV, tilt; + try { + newFOV = (Double) camSettings.get("fov"); + } catch (Exception ignored) { + newFOV = (Integer) camSettings.get("fov"); + } + try { + tilt = (Double) camSettings.get("tilt"); + } catch (Exception ignored) { + tilt = (Integer) camSettings.get("tilt"); + } + currentCamera.getProperties().setFOV(newFOV); + currentCamera.getProperties().setTilt(Rotation2d.fromDegrees(tilt)); + VisionManager.saveCurrentCameraSettings(); + SocketHandler.sendFullSettings(); + ctx.status(200); + } catch (JsonProcessingException e) { + e.printStackTrace(); + ctx.status(500); + } + } + + public static void onCalibrationStart(Context ctx) throws JsonProcessingException { + PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager; + ObjectMapper objectMapper = kObjectMapper; + var data = objectMapper.readValue(ctx.body(), Map.class); + int resolutionIndex = (Integer) data.get("resolution"); + double squareSize; + try { + squareSize = (Double) data.get("squareSize"); + } catch (Exception e) { + squareSize = (Integer) data.get("squareSize"); + } + // convert from mm to meters + pipeManager.calib3dPipe.setSquareSize(squareSize); + VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex; + VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true); + VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex); + } + + public static void onSnapshot(Context ctx) { + Calibrate3dPipeline calPipe = VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe; + + calPipe.takeSnapshot(); + + HashMap toSend = new HashMap<>(); + toSend.put("snapshotCount", calPipe.getSnapshotCount()); + toSend.put("hasEnough", calPipe.hasEnoughSnapshots()); + + ctx.json(toSend); + ctx.status(200); + } + + public static void onCalibrationEnding(Context ctx) throws JsonProcessingException { + PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager; + + var data = kObjectMapper.readValue(ctx.body(), Map.class); + double squareSize; + try { + squareSize = (Double) data.get("squareSize"); + } catch (Exception e) { + squareSize = (Integer) data.get("squareSize"); + } + pipeManager.calib3dPipe.setSquareSize(squareSize); + + System.out.println("Finishing Cal"); + if (pipeManager.calib3dPipe.hasEnoughSnapshots()) { + if (pipeManager.calib3dPipe.tryCalibration()) { + HashMap tmp = new HashMap(); + tmp.put("accuracy", pipeManager.calib3dPipe.getCalibrationAccuracy()); + ctx.json(tmp); + ctx.status(200); + } else { + System.err.println("CALFAIL"); + ctx.status(500); + } + } else { + ctx.status(201); + } + pipeManager.setCalibrationMode(false); + } + + public static void onPnpModel(Context ctx) throws JsonProcessingException { + //noinspection unchecked + List> points = kObjectMapper.readValue(ctx.body(), List.class); + try { + // each entry should be an xy pair + var pointsList = new ArrayList(); + for (List point : points) { + double x, y; + x = point.get(0).doubleValue(); + y = point.get(1).doubleValue(); + var pointToAdd = new Point3(x, y, 0.0); + pointsList.add(pointToAdd); + } + System.out.println(pointsList.toString()); + if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings instanceof StandardCVPipelineSettings) { + var settings = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings; + settings.targetCornerMat.fromList(pointsList); + } + } catch (Exception e) { + ctx.status(500); + } + } + + public static void onInstallOrUpdate(Context ctx) { + Platform p = Platform.CurrentPlatform; + try { + if (p == Platform.LINUX_RASPBIAN || p == Platform.LINUX_64) { + UploadedFile file = ctx.uploadedFile("file"); + Path filePath; + if (file != null) { + filePath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), file.getFilename()); + File target = new File(filePath.toString()); + OutputStream stream = new FileOutputStream(target); + file.getContent().transferTo(stream); + stream.close(); + } else { + filePath = Paths.get(new File(Main.class.getProtectionDomain().getCodeSource().getLocation().toURI()).getPath()); // quirk to get the current file directory + } + Helpers.setService(filePath); + ctx.status(200); + } else { + ctx.result("Only Linux Platforms Support this feature"); + ctx.status(500); + } + } catch (Exception e) { + ctx.result(e.toString()); + ctx.status(500); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java new file mode 100644 index 000000000..a888d95e3 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java @@ -0,0 +1,41 @@ +package com.chameleonvision._2.web; + +import com.chameleonvision._2.config.ConfigManager; +import io.javalin.Javalin; + +public class Server { + private static SocketHandler socketHandler; + + public static void main(int port) { + socketHandler = new SocketHandler(); + + Javalin app = Javalin.create(javalinConfig -> { + javalinConfig.showJavalinBanner = false; + javalinConfig.addStaticFiles("web"); + javalinConfig.enableCorsForAllOrigins(); + }); + app.ws("/websocket", ws -> { + ws.onConnect(ctx -> { + socketHandler.onConnect(ctx); + System.out.println("Socket Connected"); + }); + ws.onClose(ctx -> { + socketHandler.onClose(ctx); + System.out.println("Socket Disconnected"); + ConfigManager.saveGeneralSettings(); + }); + ws.onBinaryMessage(ctx -> { + socketHandler.onBinaryMessage(ctx); + }); + }); + app.post("/api/settings/general", RequestHandler::onGeneralSettings); + app.post("/api/settings/camera", RequestHandler::onCameraSettings); + app.post("/api/vision/duplicate", RequestHandler::onDuplicatePipeline); + app.post("/api/settings/startCalibration", RequestHandler::onCalibrationStart); + app.post("/api/settings/snapshot", RequestHandler::onSnapshot); + app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnding); + app.post("/api/vision/pnpModel", RequestHandler::onPnpModel); + app.post("/api/install", RequestHandler::onInstallOrUpdate); + app.start(port); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java new file mode 100644 index 000000000..7361bdb57 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java @@ -0,0 +1,306 @@ +package com.chameleonvision._2.web; + +import com.chameleonvision._2.config.CameraCalibrationConfig; +import com.chameleonvision._2.config.ConfigManager; +import com.chameleonvision._2.vision.VisionManager; +import com.chameleonvision._2.vision.VisionProcess; +import com.chameleonvision._2.vision.camera.CameraCapture; +import com.chameleonvision._2.vision.camera.CaptureStaticProperties; +import com.chameleonvision._2.vision.camera.USBCameraCapture; +import com.chameleonvision._2.vision.enums.ImageRotationMode; +import com.chameleonvision._2.vision.enums.StreamDivisor; +import com.chameleonvision._2.vision.pipeline.CVPipeline; +import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.cscore.VideoMode; +import io.javalin.websocket.WsBinaryMessageContext; +import io.javalin.websocket.WsCloseContext; +import io.javalin.websocket.WsConnectContext; +import io.javalin.websocket.WsContext; +import org.apache.commons.lang3.ArrayUtils; +import org.msgpack.jackson.dataformat.MessagePackFactory; + +import java.lang.reflect.Field; +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; + + +public class SocketHandler { + + private static List users; + private static ObjectMapper objectMapper; + + private static final Object broadcastLock = new Object(); + + SocketHandler() { + users = new ArrayList<>(); + objectMapper = new ObjectMapper(new MessagePackFactory()); + } + + void onConnect(WsConnectContext context) { + users.add(context); + sendFullSettings(); + } + + void onClose(WsCloseContext context) { + users.remove(context); + } + + @SuppressWarnings("unchecked") + void onBinaryMessage(WsBinaryMessageContext context) throws Exception { + Map deserialized = objectMapper.readValue((byte[]) ArrayUtils.toPrimitive(context.data()), + new TypeReference<>() { + }); + for (Map.Entry entry : deserialized.entrySet()) { + try { + VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess(); + CameraCapture currentCamera = currentProcess.getCamera(); + CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline(); +// System.out.println("entry.getKey()+entry.getValue()= " + entry.getKey() + entry.getValue()); + switch (entry.getKey()) { + case "driverMode": { + HashMap data = (HashMap) entry.getValue(); + currentProcess.getDriverModeSettings().exposure = (Integer) data.get("driverExposure"); + currentProcess.getDriverModeSettings().brightness = (Integer) data.get("driverBrightness"); + currentProcess.setDriverMode((Boolean) data.get("isDriver")); + + VisionManager.saveCurrentCameraDriverMode(); + break; + } + case "changeCameraName": { + currentProcess.setCameraNickname((String) entry.getValue()); + sendFullSettings(); + VisionManager.saveCurrentCameraSettings(); + break; + } + case "changePipelineName": { + currentProcess.pipelineManager.renameCurrentPipeline((String) entry.getValue()); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; + } + case "addNewPipeline": { +// HashMap data = (HashMap) entry.getValue(); + String pipeName = (String) entry.getValue(); + // TODO: add to UI selection for new 2d/3d + currentProcess.pipelineManager.addNewPipeline(pipeName); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; + } + case "command": { + switch ((String) entry.getValue()) { + case "deleteCurrentPipeline": + currentProcess.pipelineManager.deleteCurrentPipeline(); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; + case "save": + ConfigManager.saveGeneralSettings(); + VisionManager.saveAllCameras(); + System.out.println("Saved Settings"); + break; + } + // used to define all incoming commands + break; + } + case "currentCamera": { + VisionManager.setCurrentProcessByIndex((Integer) entry.getValue()); + sendFullSettings(); + break; + } + case "is3D": { + VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue()); + break; + } + case "currentPipeline": { + currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue()); + sendFullSettings(); + break; + } + case "isPNPCalibration": { + currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue()); + break; + } + case "takeCalibrationSnapshot": { + currentProcess.pipelineManager.calib3dPipe.takeSnapshot(); + } + default: { + + switch (entry.getKey()) {//Pre field value set + case "rotationMode": {//Create new CaptureStaticProperties with new width and height, reset crosshair calib + ImageRotationMode oldRot = currentPipeline.settings.rotationMode; + ImageRotationMode newRot = ImageRotationMode.class.getEnumConstants()[(Integer) entry.getValue()]; + CaptureStaticProperties prop = currentCamera.getProperties().getStaticProperties(); + int width, height; + if (oldRot.isRotated() != newRot.isRotated()) { + width = prop.mode.height; + height = prop.mode.width; + //Creates new video mode with new width and height to create new CaptureStaticProperties and applies it + currentCamera.getProperties().setStaticProperties(new CaptureStaticProperties( + new VideoMode(prop.mode.pixelFormat, width, height, prop.mode.fps), prop.fov)); + } + prop = currentCamera.getProperties().getStaticProperties(); + currentProcess.cameraStreamer.recalculateDivision(); + if (currentPipeline instanceof StandardCVPipeline) + ((StandardCVPipeline) currentPipeline).settings.point.set(prop.mode.width / 2.0, prop.mode.height / 2.0);//Reset Crosshair in single point calib + break; + } + + } + + + if (currentProcess.pipelineManager.getDriverMode()) { + setField(currentProcess.pipelineManager.driverModePipeline.settings, entry.getKey(), entry.getValue()); + } else { + setField(currentPipeline.settings, entry.getKey(), entry.getValue()); + } + + //Post field value set + switch (entry.getKey()) { + case "exposure": { + currentCamera.setExposure((Integer) entry.getValue()); + break; + } + case "brightness": { + currentCamera.setBrightness((Integer) entry.getValue()); + break; + } + case "gain": { + currentCamera.setGain((Integer) entry.getValue()); + break; + } + case "videoModeIndex": { + if (currentPipeline instanceof StandardCVPipeline) + ((StandardCVPipeline) currentPipeline).settings.point = new DoubleCouple();//This will reset the calibration + currentCamera.setVideoMode((Integer) entry.getValue()); + currentProcess.cameraStreamer.recalculateDivision(); + break; + } + case "streamDivisor": { + currentProcess.cameraStreamer.setDivisor(StreamDivisor.values()[(Integer) entry.getValue()], true); + break; + } + } + + VisionManager.saveCurrentCameraPipelines(); + break; + } + } + } catch (Exception e) { + System.err.println(e.getMessage()); + } + broadcastMessage(deserialized, context); + } + } + + private void setField(Object obj, String fieldName, Object value) { + try { + Field field = obj.getClass().getField(fieldName); + if (field.getType().isEnum()) + field.set(obj, field.getType().getEnumConstants()[(Integer) value]); + else + field.set(obj, value); + } catch (NoSuchFieldException | IllegalAccessException ex) { + ex.printStackTrace(); + } + } + + private static void broadcastMessage(Object obj, WsContext userToSkip) { + synchronized (broadcastLock) { + if (users != null) { + var userList = users; + for (WsContext user : userList) { + if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) { + continue; + } + try { + ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj)); + user.send(b); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + } + } + } + + public static void broadcastMessage(Object obj) { + broadcastMessage(obj, null);//Broadcasts the message to every user + } + + private static HashMap getOrdinalPipeline(Class cvClass) throws IllegalAccessException { + HashMap tmp = new HashMap<>(); + for (Field field : cvClass.getFields()) { // iterate over every field in CVPipelineSettings + try { + if (!field.getType().isEnum()) { // if the field is not an enum, get it based on the current pipeline + tmp.put(field.getName(), field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings)); + } else { + var ordinal = (Enum) field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings); + tmp.put(field.getName(), ordinal.ordinal()); + } + } catch (IllegalArgumentException e) { + e.printStackTrace(); + } + } + return tmp; + } + + private static HashMap getOrdinalSettings() { + HashMap tmp = new HashMap<>(); + tmp.put("teamNumber", ConfigManager.settings.teamNumber); + tmp.put("connectionType", ConfigManager.settings.connectionType.ordinal()); + tmp.put("ip", ConfigManager.settings.ip); + tmp.put("gateway", ConfigManager.settings.gateway); + tmp.put("netmask", ConfigManager.settings.netmask); + tmp.put("hostname", ConfigManager.settings.hostname); + return tmp; + } + + private static HashMap getOrdinalCameraSettings() { + HashMap tmp = new HashMap<>(); + VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess(); + USBCameraCapture currentCamera = VisionManager.getCurrentUIVisionProcess().getCamera(); + + tmp.put("fov", currentCamera.getProperties().getFOV()); + tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal()); + tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex()); + tmp.put("tilt", currentVisionProcess.getCamera().getProperties().getTilt().getDegrees()); + + List calibrations = currentCamera.getAllCalibrationData().stream() + .map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList()); + tmp.put("calibration", calibrations); + + return tmp; + } + + public static void sendFullSettings() { + //General settings + Map fullSettings = new HashMap<>(); + + VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess(); + CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline(); + + try { + fullSettings.put("settings", getOrdinalSettings()); + fullSettings.put("cameraSettings", getOrdinalCameraSettings()); + fullSettings.put("cameraList", VisionManager.getAllCameraNicknames()); + fullSettings.put("pipeline", getOrdinalPipeline(currentPipeline.settings.getClass())); + fullSettings.put("pipelineList", VisionManager.getCurrentCameraPipelineNicknames()); + fullSettings.put("resolutionList", VisionManager.getCurrentCameraResolutionList()); + fullSettings.put("port", currentProcess.cameraStreamer.getStreamPort()); + fullSettings.put("currentPipelineIndex", VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipelineIndex()); + fullSettings.put("currentCameraIndex", VisionManager.getCurrentUIVisionProcessIndex()); + } catch (IllegalAccessException e) { + System.err.println("No camera found!"); + } + broadcastMessage(fullSettings); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java index 2b5ea6f2e..b89b8ab9e 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java @@ -1,17 +1,12 @@ package com.chameleonvision.common.configuration; -import com.chameleonvision.common.server.configuration.MainConfig; - public class ConfigManager { private final ConfigFolder rootFolder; - final MainConfig mainConfig; protected ConfigManager() { rootFolder = new ConfigFolder(""); - - mainConfig = MainConfig.getInstance(); } private static class SingletonHolder { diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java new file mode 100644 index 000000000..067b5098f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java @@ -0,0 +1,4 @@ +package com.chameleonvision.common.datatransfer; + +public interface DataConsumer { +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java new file mode 100644 index 000000000..e88f0d608 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java @@ -0,0 +1,4 @@ +package com.chameleonvision.common.datatransfer; + +public interface DataProvider { +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java new file mode 100644 index 000000000..1a158c07c --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java @@ -0,0 +1,75 @@ +package com.chameleonvision.common.datatransfer.networktables; + +import com.chameleonvision.common.scripting.ScriptEventType; +import com.chameleonvision.common.scripting.ScriptManager; +import edu.wpi.first.networktables.LogMessage; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +import java.util.function.Consumer; + +public class NetworkTablesManager { + + private NetworkTablesManager() {} + + private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); + + public static final String kRootTableName = "/chameleon-vision"; + public static final NetworkTable kRootTable = NetworkTableInstance.getDefault().getTable(kRootTableName); + + public static boolean isServer = false; + + private static int getTeamNumber() { + // TODO: FIX + return 0; +// return ConfigManager.settings.teamNumber; + } + + private static class NTLogger implements Consumer { + + private boolean hasReportedConnectionFailure = false; + + @Override + public void accept(LogMessage logMessage) { + if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) { + System.err.println("NT Connection has failed! Will retry in background."); + hasReportedConnectionFailure = true; + } else if (logMessage.message.contains("connected")) { + System.out.println("NT Connected!"); + hasReportedConnectionFailure = false; + ScriptManager.queueEvent(ScriptEventType.kNTConnected); + } + } + } + + static { + NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages + } + + public static void setClientMode(String host) { + isServer = false; + System.out.println("Starting NT Client"); + ntInstance.stopServer(); + if (host != null) { + ntInstance.startClient(host); + } else { + ntInstance.startClientTeam(getTeamNumber()); + if(ntInstance.isConnected()) { + System.out.println("[NetworkTablesManager] Connected to the robot!"); + } else { + System.out.println("[NetworkTablesManager] Could NOT to the robot! Will retry in the background..."); + } + } + } + + public static void setTeamClientMode() { + setClientMode(null); + } + + public static void setServerMode() { + isServer = true; + System.out.println("Starting NT Server"); + ntInstance.stopClient(); + ntInstance.startServer(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java b/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java new file mode 100644 index 000000000..47ba1ae1c --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java @@ -0,0 +1,21 @@ +package com.chameleonvision.common.logging; + +public class DebugLogger { + + private final boolean verbose; + + public DebugLogger(boolean verbose) { + this.verbose = verbose; + } + + public void printInfo(String infoMessage) { + if (verbose) { + System.out.println(infoMessage); + } + } + + public void printInfo(String smallInfo, String largeInfo) { + System.out.println(verbose ? String.format("%s - %s" , smallInfo, largeInfo) : smallInfo); + } + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/network/LinuxNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java similarity index 98% rename from chameleon-server/src/main/java/com/chameleonvision/common/network/LinuxNetworking.java rename to chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java index 13628893b..64adcaa95 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/network/LinuxNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.network; +package com.chameleonvision.common.networking; import org.apache.commons.io.FileUtils; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkInterface.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkInterface.java similarity index 97% rename from chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkInterface.java rename to chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkInterface.java index 99b9bc91a..cbb1267b9 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkInterface.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkInterface.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.network; +package com.chameleonvision.common.networking; import java.net.InterfaceAddress; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkManager.java similarity index 91% rename from chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkManager.java rename to chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkManager.java index a3958ca90..ac633f8ce 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkManager.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.network; +package com.chameleonvision.common.networking; public class NetworkManager { private NetworkManager() {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkMode.java similarity index 51% rename from chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkMode.java rename to chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkMode.java index aa979670c..be0bd28c2 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/network/NetworkMode.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkMode.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.network; +package com.chameleonvision.common.networking; public enum NetworkMode { DHCP, diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/network/SysNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/SysNetworking.java similarity index 95% rename from chameleon-server/src/main/java/com/chameleonvision/common/network/SysNetworking.java rename to chameleon-server/src/main/java/com/chameleonvision/common/networking/SysNetworking.java index 212b79eb9..ef2116bad 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/network/SysNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/SysNetworking.java @@ -1,6 +1,6 @@ -package com.chameleonvision.common.network; +package com.chameleonvision.common.networking; -import com.chameleonvision.common.server.util.ShellExec; +import com.chameleonvision.common.util.ShellExec; import java.io.IOException; import java.net.InetAddress; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java new file mode 100644 index 000000000..4a278603c --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java @@ -0,0 +1,14 @@ +package com.chameleonvision.common.scripting; + +public enum ScriptCommandType { + kDefault(""), + kBashScript("bash"), + kPythonScript("python"), + kPython3Script("python3"); + + public final String value; + + ScriptCommandType(String value) { + this.value = value; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java new file mode 100644 index 000000000..acf9127aa --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java @@ -0,0 +1,23 @@ +package com.chameleonvision.common.scripting; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; + +public class ScriptConfig { + public final ScriptEventType eventType; + public final String command; + + public ScriptConfig(ScriptEventType eventType) { + this.eventType = eventType; + this.command = ""; + } + + @JsonCreator + public ScriptConfig( + @JsonProperty("eventType") ScriptEventType eventType, + @JsonProperty("command") String command + ) { + this.eventType = eventType; + this.command = command; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java new file mode 100644 index 000000000..255b19e0f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java @@ -0,0 +1,33 @@ +package com.chameleonvision.common.scripting; + + +import com.chameleonvision.common.logging.DebugLogger; +import com.chameleonvision.common.util.ShellExec; + +import java.io.IOException; + +public class ScriptEvent { + private static final DebugLogger logger = new DebugLogger(true); + private static final ShellExec executor = new ShellExec(true, true); + + public final ScriptConfig config; + + public ScriptEvent(ScriptConfig config) { + this.config = config; + } + + public int run() throws IOException { + int retVal = executor.executeBashCommand(config.command); + + String output = executor.getOutput(); + String error = executor.getError(); + + if (!error.isEmpty()) { + System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error); + } else if (!output.isEmpty()) { + logger.printInfo(String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output)); + } + logger.printInfo(String.format("Script for %s ran with command line: \"%s\", exit code: %d, output: %s, error: %s\n", config.eventType.name(), config.command, retVal, output, error)); + return retVal; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java new file mode 100644 index 000000000..5292df006 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java @@ -0,0 +1,21 @@ +package com.chameleonvision.common.scripting; + +public enum ScriptEventType { + kProgramInit("Program Init"), + kProgramExit("Program Exit"), + kNTConnected("NT Connected"), + kLEDOn("LED On"), + kLEDOff("LED Off"), + kEnterDriverMode("Enter Driver Mode"), + kExitDriverMode("Exit Driver Mode"), + kFoundTarget("Found Target"), + kFoundMultipleTarget("Found Multiple Target"), + kLostTarget("Lost Target"), + kPipelineLag("Pipeline Lag"); + + public final String value; + + ScriptEventType(String value) { + this.value = value; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java new file mode 100644 index 000000000..442275bcc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java @@ -0,0 +1,126 @@ +package com.chameleonvision.common.scripting; + +import com.chameleonvision.common.logging.DebugLogger; +import com.chameleonvision.common.util.LoopingRunnable; +import com.chameleonvision.common.util.Platform; +import com.chameleonvision.common.util.file.JacksonUtils; + +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.LinkedBlockingDeque; + +public class ScriptManager { + + private static DebugLogger logger = new DebugLogger(true); + + private ScriptManager() { + } + + private static final List events = new ArrayList<>(); + private static final LinkedBlockingDeque queuedEvents = new LinkedBlockingDeque<>(25); + + public static void initialize() { + ScriptConfigManager.initialize(); + if (ScriptConfigManager.fileExists()) { + for (ScriptConfig scriptConfig : ScriptConfigManager.loadConfig()) { + ScriptEvent scriptEvent = new ScriptEvent(scriptConfig); + events.add(scriptEvent); + } + + new Thread(new ScriptRunner(10L)).start(); + } else { + System.err.println("Something went wrong initializing scripts! Events will not run."); + } + } + + private static class ScriptRunner extends LoopingRunnable { + + ScriptRunner(Long loopTimeMs) { + super(loopTimeMs); + } + + @Override + protected void process() { + try { + + handleEvent(queuedEvents.takeFirst()); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + + private void handleEvent(ScriptEventType eventType) { + var toRun = events.parallelStream().filter(e -> e.config.eventType == eventType).findFirst().orElse(null); + if (toRun != null) { + try { + toRun.run(); + } catch (IOException e) { + System.err.printf("Failed to run script for event: %s, exception below.\n%s\n", eventType.name(), e.getMessage()); + } + } + } + } + + protected static class ScriptConfigManager { + +// protected static final Path scriptConfigPath = Paths.get(ConfigManager.SettingsPath.toString(), "scripts.json"); + static final Path scriptConfigPath = Paths.get(""); // TODO: FIX + + private ScriptConfigManager() { + } + + static boolean fileExists() { + return Files.exists(scriptConfigPath); + } + + public static void initialize() { + if (!fileExists()) { + List eventsConfig = new ArrayList<>(); + for (var eventType : ScriptEventType.values()) { + eventsConfig.add(new ScriptConfig(eventType)); + } + + try { + JacksonUtils.serializer(scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true); + } catch (IOException e) { + e.printStackTrace(); + } + } + } + + static List loadConfig() { + try { + var raw = JacksonUtils.deserialize(scriptConfigPath, ScriptConfig[].class); + if (raw != null) { + return List.of(raw); + } + } catch (IOException e) { + e.printStackTrace(); + } + return new ArrayList<>(); + } + + protected static void deleteConfig() { + try { + Files.delete(scriptConfigPath); + } catch (IOException e) { + // + } + } + } + + public static void queueEvent(ScriptEventType eventType) { + if (!Platform.CurrentPlatform.isWindows()) { + try { + queuedEvents.putLast(eventType); + logger.printInfo("Queued event: " + eventType.name()); + } catch (InterruptedException e) { + System.err.println("Failed to add event to queue: " + eventType.name()); + } + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/MainConfig.java b/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/MainConfig.java deleted file mode 100644 index 64d1ca481..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/MainConfig.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.chameleonvision.common.server.configuration; - -import com.chameleonvision.common.configuration.ConfigFile; - -public class MainConfig extends ConfigFile { - - public int teamNumber = 0; - public boolean ntServer = false; - - private MainConfig() { - super("general"); - } - - private static class SingletonHolder { - private static final MainConfig INSTANCE = new MainConfig(); - } - - public static MainConfig getInstance() { - return SingletonHolder.INSTANCE; - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/NetworkConfig.java b/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/NetworkConfig.java deleted file mode 100644 index 2e40b825b..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/server/configuration/NetworkConfig.java +++ /dev/null @@ -1,23 +0,0 @@ -package com.chameleonvision.common.server.configuration; - -import com.chameleonvision.common.configuration.ConfigFile; -import com.chameleonvision.common.network.NetworkMode; - -public class NetworkConfig extends ConfigFile { - - public NetworkMode networkMode = NetworkMode.DHCP; - public String ip = ""; - public String hostname = "chameleon-vision"; - - private NetworkConfig() { - super("network"); - } - - private static class SingletonHolder { - private static final NetworkConfig INSTANCE = new NetworkConfig(); - } - - public static NetworkConfig getInstance() { - return SingletonHolder.INSTANCE; - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExecutor.java b/chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExecutor.java deleted file mode 100644 index bf79ed2b9..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExecutor.java +++ /dev/null @@ -1,45 +0,0 @@ -package com.chameleonvision.common.server.util; - -import org.apache.commons.exec.*; - -import java.io.ByteArrayOutputStream; -import java.io.OutputStream; - -// TODO: Finish me! -@SuppressWarnings({"FieldCanBeLocal", "unused"}) -public class ShellExecutor { - - private final Executor executor; - private final ExecuteWatchdog watchdog; - private final DefaultExecuteResultHandler resultHandler = new DefaultExecuteResultHandler(); - private final OutputStream stdOutStream = new ByteArrayOutputStream(); - private final OutputStream stdErrStream = new ByteArrayOutputStream(); - private final boolean block; - - public ShellExecutor(String command, boolean block, int timeoutMillis, String... args) { - this.block = block; - - CommandLine cmdLine = new CommandLine(command); - cmdLine.addArguments(args); - - watchdog = new ExecuteWatchdog(timeoutMillis); - executor = new DefaultExecutor(); - executor.setWatchdog(watchdog); - executor.setStreamHandler(new PumpStreamHandler(stdOutStream, stdErrStream)); - } - -// public int execute() { -// if () -// } - - public String getStdOut() { - if (!watchdog.isWatching()) { - return executor.toString(); - } - return ""; - } - - public String getStdErr() { - return ""; - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/ColorHelper.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/ColorHelper.java new file mode 100644 index 000000000..1c264cfc3 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/ColorHelper.java @@ -0,0 +1,11 @@ +package com.chameleonvision.common.util; + +import org.opencv.core.Scalar; + +import java.awt.*; + +public class ColorHelper { + public static Scalar colorToScalar(Color color) { + return new Scalar(color.getBlue(), color.getGreen(), color.getRed()); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/LoopingRunnable.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/LoopingRunnable.java new file mode 100644 index 000000000..f81cca41f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/LoopingRunnable.java @@ -0,0 +1,37 @@ +package com.chameleonvision.common.util; + +/** + * A thread that tries to run at a specified loop time + */ +public abstract class LoopingRunnable implements Runnable { + protected volatile Long loopTimeMs; + + protected abstract void process(); + + public LoopingRunnable(Long loopTimeMs) { + this.loopTimeMs = loopTimeMs; + } + + @Override + public void run() { + while(!Thread.interrupted()) { + var now = System.currentTimeMillis(); + + // Do the thing + process(); + + // sleep for the remaining time + var timeElapsed = System.currentTimeMillis() - now; + var delta = loopTimeMs - timeElapsed; + try { + if(delta > 0.0) { + + Thread.sleep(delta, 0); + + } else { + Thread.sleep(1); + } + } catch (Exception ignored) {} + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/MemoryManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/MemoryManager.java new file mode 100644 index 000000000..254b3d714 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/MemoryManager.java @@ -0,0 +1,66 @@ +package com.chameleonvision.common.util; + +public class MemoryManager { + + private static final long MEGABYTE_FACTOR = 1024L * 1024L; + + private int collectionThreshold; + private long collectionPeriodMillis = -1; + + private double lastUsedMb = 0; + private long lastCollectionMillis = 0; + + public MemoryManager(int collectionThreshold) { + this.collectionThreshold = collectionThreshold; + } + + public MemoryManager(int collectionThreshold, long collectionPeriodMillis) { + this.collectionThreshold = collectionThreshold; + this.collectionPeriodMillis = collectionPeriodMillis; + } + + public void setCollectionThreshold(int collectionThreshold) { + this.collectionThreshold = collectionThreshold; + } + + public void setCollectionPeriodMillis(long collectionPeriodMillis) { + this.collectionPeriodMillis = collectionPeriodMillis; + } + + private static long getUsedMemory() { + return Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory(); + } + + private static double getUsedMemoryMB() { + return ((double) getUsedMemory() / MEGABYTE_FACTOR); + } + + private void collect() { + System.gc(); + System.runFinalization(); + } + + public void run() { + run(false); + } + + public void run(boolean print) { + var usedMem = getUsedMemoryMB(); + + if (usedMem != lastUsedMb) { + lastUsedMb = usedMem; + if (print) System.out.printf("Memory usage: %.2fMB\n", usedMem); + } + + boolean collectionThresholdPassed = usedMem >= collectionThreshold; + boolean collectionPeriodPassed = collectionPeriodMillis != -1 && (System.currentTimeMillis() - lastCollectionMillis >= collectionPeriodMillis); + + if (collectionThresholdPassed || collectionPeriodPassed) { + collect(); + lastCollectionMillis = System.currentTimeMillis(); + if (print) { + System.out.printf("Garbage collected at %.2fMB\n", usedMem); + } + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/Platform.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/Platform.java similarity index 98% rename from chameleon-server/src/main/java/com/chameleonvision/common/server/util/Platform.java rename to chameleon-server/src/main/java/com/chameleonvision/common/util/Platform.java index dc38da09d..263c7439f 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/Platform.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/Platform.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.server.util; +package com.chameleonvision.common.util; import edu.wpi.first.wpiutil.RuntimeDetector; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExec.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/ShellExec.java similarity index 99% rename from chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExec.java rename to chameleon-server/src/main/java/com/chameleonvision/common/util/ShellExec.java index 16ee97af0..c304e3d9c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/server/util/ShellExec.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/ShellExec.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.server.util; +package com.chameleonvision.common.util; import java.io.*; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java new file mode 100644 index 000000000..415219847 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java @@ -0,0 +1,51 @@ +package com.chameleonvision.common.util.file; + +import com.chameleonvision.common.logging.DebugLogger; +import com.chameleonvision.common.util.Platform; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.attribute.PosixFileAttributes; +import java.nio.file.attribute.PosixFilePermission; +import java.util.Arrays; +import java.util.HashSet; +import java.util.Set; + +public class FileUtils { + private static DebugLogger logger = new DebugLogger(true); + private static final Set allReadWriteExecutePerms = new HashSet<>(Arrays.asList(PosixFilePermission.values())); + + public static void setFilePerms(Path path) throws IOException { + if (!Platform.CurrentPlatform.isWindows()) { + File thisFile = path.toFile(); + Set perms = Files.readAttributes(path, PosixFileAttributes.class).permissions(); + if (!perms.equals(allReadWriteExecutePerms)) { + logger.printInfo("Setting perms on" + path.toString()); + Files.setPosixFilePermissions(path, perms); + if (thisFile.isDirectory()) { + for (File subfile : thisFile.listFiles()) { + setFilePerms(subfile.toPath()); + } + } + } + } + } + + public static void setAllPerms(Path path) { + if (!Platform.CurrentPlatform.isWindows()) { + String command = String.format("chmod 777 -R %s", path.toString()); + try { + Process p = Runtime.getRuntime().exec(command); + p.waitFor(); + + } catch (Exception e) { + e.printStackTrace(); + } + } else { + // TODO file perms on Windows + System.out.println("File permission setting not available on Windows. Not changing file permissions."); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/JacksonUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/JacksonUtils.java new file mode 100644 index 000000000..742e88829 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/JacksonUtils.java @@ -0,0 +1,74 @@ +package com.chameleonvision.common.util.file; + +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.deser.std.StdDeserializer; +import com.fasterxml.jackson.databind.json.JsonMapper; +import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator; +import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator; +import com.fasterxml.jackson.databind.module.SimpleModule; +import com.fasterxml.jackson.databind.ser.std.StdSerializer; + +import java.io.File; +import java.io.FileDescriptor; +import java.io.FileOutputStream; +import java.io.IOException; +import java.nio.file.Path; + +public class JacksonUtils { + public static void serializer(Path path, T object) throws IOException { + serializer(path, object, false); + } + + public static void serializer(Path path, T object, boolean forceSync) throws IOException { + PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(object.getClass()).build(); + ObjectMapper objectMapper = JsonMapper.builder().activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT).build(); + String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object); + saveJsonString(json, path, forceSync); + } + + public static T deserialize(Path path, Class ref) throws IOException { + PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build(); + ObjectMapper objectMapper = JsonMapper.builder().activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT).build(); + File jsonFile = new File(path.toString()); + if (jsonFile.exists() && jsonFile.length() > 0) { + return objectMapper.readValue(jsonFile, ref); + } + return null; + } + + public static T deserialize(Path path, Class ref, StdDeserializer deserializer) throws IOException { + ObjectMapper objectMapper = new ObjectMapper(); + SimpleModule module = new SimpleModule(); + module.addDeserializer(ref, deserializer); + objectMapper.registerModule(module); + + File jsonFile = new File(path.toString()); + if (jsonFile.exists() && jsonFile.length() > 0) { + return objectMapper.readValue(jsonFile, ref); + } + return null; + } + public static void serialize(Path path, T object, Class ref, StdSerializer serializer) throws IOException { + serialize(path, object, ref, serializer, false); + } + + public static void serialize(Path path, T object, Class ref, StdSerializer serializer, boolean forceSync) throws IOException { + ObjectMapper objectMapper = new ObjectMapper(); + SimpleModule module = new SimpleModule(); + module.addSerializer(ref, serializer); + objectMapper.registerModule(module); + String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object); + saveJsonString(json, path, forceSync); + } + + private static void saveJsonString(String json, Path path, boolean forceSync) throws IOException { + FileOutputStream fileOutputStream = new FileOutputStream(path.toFile()); + fileOutputStream.write(json.getBytes()); + fileOutputStream.flush(); + if (forceSync) { + FileDescriptor fileDescriptor = fileOutputStream.getFD(); + fileDescriptor.sync(); + } + fileOutputStream.close(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/math/IPUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/IPUtils.java new file mode 100644 index 000000000..2c189a97d --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/IPUtils.java @@ -0,0 +1,38 @@ +package com.chameleonvision.common.util.math; + +import java.util.ArrayList; +import java.util.List; + +public class IPUtils { + public static boolean isValidIPV4(final String ip) { + String PATTERN = "^((0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)\\.){3}(0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)$"; + + return ip.matches(PATTERN); + } + + public static List getDigitBytes(int num) { + List digits = new ArrayList<>(); + collectDigitBytes(num, digits); + return digits; + } + + private static void collectDigitBytes(int num, List digits) { + if (num / 10 > 0) { + collectDigitBytes( num / 10, digits); + } + digits.add((byte) (num % 10)); + } + + public static List getDigits(int num) { + List digits = new ArrayList<>(); + collectDigits(num, digits); + return digits; + } + + private static void collectDigits(int num, List digits) { + if(num / 10 > 0) { + collectDigits(num / 10, digits); + } + digits.add(num % 10); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java new file mode 100644 index 000000000..01d4f3bf8 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java @@ -0,0 +1,30 @@ +package com.chameleonvision.common.util.math; + +import org.apache.commons.math3.util.FastMath; + +public class MathUtils { + MathUtils() {} + + public static double sigmoid(Number x){ + double bias = 0; + double a = 5; + double b = -0.05; + double k = 200; + + if (x.doubleValue() < 50){ + bias = -1.338; + } + + return ((k / (1 + Math.pow(Math.E,(a + (b * x.doubleValue()))))) + bias); + } + + public static double toSlope(Number angle){ + return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90)); + } + + public static double roundTo(double value, int to) { + double toMult = Math.pow(10, to); + return (double)Math.round(value * toMult) / toMult; + } + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java new file mode 100644 index 000000000..a69d6e439 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java @@ -0,0 +1,12 @@ +package com.chameleonvision.common.util.numbers; + +public class DoubleCouple extends NumberCouple { + + public DoubleCouple() { + super(0.0, 0.0); + } + + public DoubleCouple(Double first, Double second) { + super(first, second); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/IntegerCouple.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/IntegerCouple.java new file mode 100644 index 000000000..f8209e634 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/IntegerCouple.java @@ -0,0 +1,12 @@ +package com.chameleonvision.common.util.numbers; + +public class IntegerCouple extends NumberCouple { + + public IntegerCouple() { + super(0, 0); + } + + public IntegerCouple(Integer first, Integer second) { + super(first, second); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java new file mode 100644 index 000000000..3fd0ee3cc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java @@ -0,0 +1,33 @@ +package com.chameleonvision.common.util.numbers; + +public class NumberCouple { + + private T first; + private T second; + + public NumberCouple(T first, T second) { + this.first = first; + this.second = second; + } + + public void setFirst(T first) { + this.first = first; + } + + public T getFirst() { + return first; + } + + public void setSecond(T second) { + this.second = second; + } + + public T getSecond() { + return second; + } + + public void set(T first, T second) { + this.first = first; + this.second = second; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/camera/USBCamera.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/camera/USBCamera.java index c792006e4..16f25d83b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/camera/USBCamera.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/camera/USBCamera.java @@ -1,6 +1,6 @@ package com.chameleonvision.common.vision.base.camera; -import com.chameleonvision.common.vision.base.capture.USBFrameProvider; +import com.chameleonvision.common.vision.base.frame.provider.USBFrameProvider; public class USBCamera extends USBFrameProvider { } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/FrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/FrameConsumer.java index 2374a21f6..57fd4351c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/FrameConsumer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/FrameConsumer.java @@ -1,5 +1,5 @@ package com.chameleonvision.common.vision.base.frame; public interface FrameConsumer { - + void consume(Frame frame); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/consumer/MJPGFrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/consumer/MJPGFrameConsumer.java new file mode 100644 index 000000000..92f719263 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/consumer/MJPGFrameConsumer.java @@ -0,0 +1,12 @@ +package com.chameleonvision.common.vision.base.frame.consumer; + +import com.chameleonvision.common.vision.base.frame.Frame; +import com.chameleonvision.common.vision.base.frame.FrameConsumer; +import org.apache.commons.lang3.NotImplementedException; + +public class MJPGFrameConsumer implements FrameConsumer { + @Override + public void consume(Frame frame) { + throw new NotImplementedException(""); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/FileFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/FileFrameProvider.java similarity index 84% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/FileFrameProvider.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/FileFrameProvider.java index 09df97e15..0747388cc 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/FileFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/FileFrameProvider.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.vision.base.capture; +package com.chameleonvision.common.vision.base.frame.provider; import com.chameleonvision.common.vision.base.frame.Frame; import com.chameleonvision.common.vision.base.frame.FrameProvider; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/NetworkFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/NetworkFrameProvider.java similarity index 84% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/NetworkFrameProvider.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/NetworkFrameProvider.java index a236ad8a7..f28340aa1 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/NetworkFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/NetworkFrameProvider.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.vision.base.capture; +package com.chameleonvision.common.vision.base.frame.provider; import com.chameleonvision.common.vision.base.frame.Frame; import com.chameleonvision.common.vision.base.frame.FrameProvider; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/USBFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/USBFrameProvider.java similarity index 84% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/USBFrameProvider.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/USBFrameProvider.java index 0b96d2f94..78462479c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/capture/USBFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/frame/provider/USBFrameProvider.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.vision.base.capture; +package com.chameleonvision.common.vision.base.frame.provider; import com.chameleonvision.common.vision.base.frame.Frame; import com.chameleonvision.common.vision.base.frame.FrameProvider; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/Pipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/CVPipe.java similarity index 77% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/Pipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/CVPipe.java index 7a9610f06..6fe77a4d6 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/Pipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/CVPipe.java @@ -9,9 +9,14 @@ import java.util.function.Function; * @param Input type for the pipe * @param Output type for the pipe */ -public abstract class Pipe implements Function> { +public abstract class CVPipe implements Function> { - private PipeResult result = new PipeResult<>(); + protected PipeResult result = new PipeResult<>(); + protected P params; + + public void setParams(P params) { + this.params = params; + } /** * Runs the process for the pipe. diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/DummyPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/DummyPipeline.java new file mode 100644 index 000000000..1c5d74089 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/DummyPipeline.java @@ -0,0 +1,35 @@ +package com.chameleonvision.common.vision.base.pipeline; + +import com.chameleonvision.common.vision.base.pipeline.pipe.ResizeImagePipe; +import com.chameleonvision.common.vision.base.pipeline.pipe.RotateImagePipe; +import edu.wpi.cscore.CameraServerCvJNI; +import org.opencv.core.CvType; +import org.opencv.core.Mat; + +import java.io.IOException; + +/** + * This class exists for the sole purpose of showing how pipes would interact in a pipeline + */ +public class DummyPipeline { + private static ResizeImagePipe resizePipe = new ResizeImagePipe(); + private static RotateImagePipe rotatePipe = new RotateImagePipe(); + + public static void main(String[] args) { + try { + CameraServerCvJNI.forceLoad(); + } catch (UnsatisfiedLinkError | IOException e) { + throw new RuntimeException("Failed to load JNI Libraries!"); + } + + // obviously not a useful test, purely for example. + Mat fakeCameraMat = new Mat(640, 480, CvType.CV_8UC3); + + PipeResult resizeResult = resizePipe.apply(fakeCameraMat); + PipeResult rotateResult = rotatePipe.apply(resizeResult.result); + + long fullTime = resizeResult.nanosElapsed + rotateResult.nanosElapsed; + System.out.println(fullTime / 1.0e+6 + "ms elapsed"); + } + +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/ResizeImagePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/ResizeImagePipe.java index b17324026..e6346031d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/ResizeImagePipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/ResizeImagePipe.java @@ -1,16 +1,31 @@ package com.chameleonvision.common.vision.base.pipeline.pipe; -import com.chameleonvision.common.vision.base.pipeline.Pipe; +import com.chameleonvision.common.vision.base.pipeline.CVPipe; +import com.chameleonvision.common.vision.base.pipeline.pipe.params.ResizeImageParams; import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; -public class ResizeImagePipe extends Pipe { +/** + * Pipe that resizes an image to a given resolution + */ +public class ResizeImagePipe extends CVPipe { public ResizeImagePipe() { - + setParams(ResizeImageParams.DEFAULT); } + public ResizeImagePipe(ResizeImageParams params) { + setParams(params); + } + + /** + * Process this pipe + * @param in {@link Mat} to be resized + * @return Resized {@link Mat} + */ @Override protected Mat process(Mat in) { - return null; + Imgproc.resize(in, in, params.getSize()); + return in; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/RotateImagePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/RotateImagePipe.java new file mode 100644 index 000000000..9b1747693 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/RotateImagePipe.java @@ -0,0 +1,31 @@ +package com.chameleonvision.common.vision.base.pipeline.pipe; + +import com.chameleonvision.common.vision.base.pipeline.CVPipe; +import com.chameleonvision.common.vision.base.pipeline.pipe.params.RotateImageParams; +import org.opencv.core.Core; +import org.opencv.core.Mat; + +/** + * Pipe that rotates an image to a given orientation + */ +public class RotateImagePipe extends CVPipe { + + public RotateImagePipe() { + setParams(RotateImageParams.DEFAULT); + } + + public RotateImagePipe(RotateImageParams params) { + setParams(params); + } + + /** + * Process this pipe + * @param in {@link Mat} to be rotated + * @return Rotated {@link Mat} + */ + @Override + protected Mat process(Mat in) { + Core.rotate(in, in, params.rotation.value); + return in; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/javacv/GPUResizeImagePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/javacv/GPUResizeImagePipe.java deleted file mode 100644 index 46c6216c1..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/javacv/GPUResizeImagePipe.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.chameleonvision.common.vision.base.pipeline.pipe.javacv; - -import com.chameleonvision.common.vision.base.pipeline.Pipe; -import org.bytedeco.opencv.opencv_core.GpuMat; - -public class GPUResizeImagePipe extends Pipe { - - public GPUResizeImagePipe() { - - } - - @Override - protected GpuMat process(GpuMat in) { - return null; - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/ResizeImageParams.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/ResizeImageParams.java new file mode 100644 index 000000000..98973b513 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/ResizeImageParams.java @@ -0,0 +1,26 @@ +package com.chameleonvision.common.vision.base.pipeline.pipe.params; + +import org.opencv.core.Size; + +public class ResizeImageParams { + + public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240); + + private Size size; + public int width; + public int height; + + public ResizeImageParams() { + this(DEFAULT.width, DEFAULT.height); + } + + public ResizeImageParams(int width, int height) { + this.width = width; + this.height = height; + size = new Size(new double[]{width, height}); + } + + public Size getSize() { + return size; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/RotateImageParams.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/RotateImageParams.java new file mode 100644 index 000000000..49872a834 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/pipeline/pipe/params/RotateImageParams.java @@ -0,0 +1,33 @@ +package com.chameleonvision.common.vision.base.pipeline.pipe.params; + +public class RotateImageParams { + + public static RotateImageParams DEFAULT = new RotateImageParams(ImageRotation.DEG_0); + + public ImageRotation rotation; + + public RotateImageParams() { + rotation = DEFAULT.rotation; + } + + public RotateImageParams(ImageRotation rotation) { + this.rotation = rotation; + } + + public enum ImageRotation { + DEG_0(-1), + DEG_90(0), + DEG_180(1), + DEG_270(2); + + public final int value; + + ImageRotation(int value) { + this.value = value; + } + + public boolean isRotated() { + return this.value==DEG_90.value || this.value==DEG_270.value; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/AsyncMjpgStreamer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/AsyncMjpgStreamer.java deleted file mode 100644 index f59486fa4..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/AsyncMjpgStreamer.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.chameleonvision.common.vision.base.stream; - -public class AsyncMjpgStreamer extends MjpgStreamer { -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/MjpgStreamer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/MjpgStreamer.java deleted file mode 100644 index decd48963..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/MjpgStreamer.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.chameleonvision.common.vision.base.stream; - -public class MjpgStreamer { -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/Streamer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/Streamer.java deleted file mode 100644 index 6bb6450e2..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/base/stream/Streamer.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.chameleonvision.common.vision.base.stream; - -public interface Streamer { -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/OpenCVWrapper.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/OpenCVWrapper.java deleted file mode 100644 index 4457f598d..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/OpenCVWrapper.java +++ /dev/null @@ -1,4 +0,0 @@ -package com.chameleonvision.common.vision.opencv; - -public class OpenCVWrapper { -}