diff --git a/.gitignore b/.gitignore index bdd8f087..49e0d362 100644 --- a/.gitignore +++ b/.gitignore @@ -98,3 +98,4 @@ Software/GroundTruthAnnotator/yolo/labels/*.cache # Dev scripts cache Dev/scripts/__pycache__ packaging/pitrac +*.save diff --git a/Software/LMSourceCode/ImageProcessing/golf_sim_config.json b/Software/LMSourceCode/ImageProcessing/golf_sim_config.json index 4a89fce8..932ece5b 100644 --- a/Software/LMSourceCode/ImageProcessing/golf_sim_config.json +++ b/Software/LMSourceCode/ImageProcessing/golf_sim_config.json @@ -118,7 +118,7 @@ "kUseBestCircleRefinement": "0", "kUseCLAHEProcessing": "1", "kUseDynamicRadiiAdjustment": "0", - "kImageTypeToProcessWithYOLO": "1" + "kImageTypeToProcessWithYOLO": "1" }, "ball_position": { "kExpectedBallRadiusPixelsAt40cm": "87", @@ -173,37 +173,17 @@ "0.481" ], "kCalibrationRigType": "1", - "kCustomCalibrationRigPositionFromCamera1": [ - -0.12, - -0.28, - 0.44 - ], - "kCustomCalibrationRigPositionFromCamera2": [ - "0.00", - "0.095", - "0.435" - ], + "kCustomCalibrationRigPositionFromCamera1": [-0.12, -0.28, 0.44], + "kCustomCalibrationRigPositionFromCamera2": ["0.00", "0.095", "0.435"], "kNumberOfCalibrationFailuresToTolerate": "4", "kNumberPicturesForFocalLengthAverage": "6", "kTestAutoCalibrationFileName": "/usr/share/pitrac/calibration/checkerboard.png" }, "cameras": { "3_6Lens_kCamera1CalibrationMatrix": [ - [ - "1748.506644661262953", - "0.0", - "632.5374407393054526" - ], - [ - "0.0", - "1743.341748687922745", - "407.5677927449370941" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1748.506644661262953", "0.0", "632.5374407393054526"], + ["0.0", "1743.341748687922745", "407.5677927449370941"], + ["0.0", "0.0", "1.0"] ], "3_6Lens_kCamera1DistortionVector": [ "-0.5323350228082535107", @@ -213,21 +193,9 @@ "0.0" ], "3_6Lens_kCamera2CalibrationMatrix": [ - [ - "1065.366451276604266", - "0.0", - "715.3187116996945178" - ], - [ - "0.0", - "1068.642040947899886", - "509.4612905764302582" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1065.366451276604266", "0.0", "715.3187116996945178"], + ["0.0", "1068.642040947899886", "509.4612905764302582"], + ["0.0", "0.0", "1.0"] ], "3_6Lens_kCamera2DistortionVector": [ "-0.4854175692996230418", @@ -246,26 +214,11 @@ "0.095", "0.435" ], - "kCamera1Angles": [ - 18.7191101, - -24.17996647 - ], + "kCamera1Angles": [18.7191101, -24.17996647], "kCamera1CalibrationMatrix": [ - [ - "1833.5291988027575", - "0.0", - "697.2791579239232" - ], - [ - "0.0", - "1832.2499845181273", - "513.0904087097207" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1833.5291988027575", "0.0", "697.2791579239232"], + ["0.0", "1832.2499845181273", "513.0904087097207"], + ["0.0", "0.0", "1.0"] ], "kCamera1Contrast": "1.0", "kCamera1DistortionVector": [ @@ -279,35 +232,16 @@ "kCamera1Gain": "2", "kCamera1Saturation": "1", "kCamera1HighFPSGain": "15.0", - "kCamera1PositionsFromExpectedBallMeters": [ - "-0.200", - "-0.234", - "0.54" - ], + "kCamera1PositionsFromExpectedBallMeters": ["-0.200", "-0.234", "0.54"], "kCamera1StillShutterTimeuS": "40000", "kCamera1XOffsetForTilt": "0", "kCamera1YOffsetForTilt": "0", - "kCamera2Angles": [ - -2.063732969, - 3.830271852 - ], + "kCamera2Angles": [-2.063732969, 3.830271852], "kCamera2CalibrateOrLocationGain": "1.0", "kCamera2CalibrationMatrix": [ - [ - "2340.2520648903665", - "0.0", - "698.4611375636877" - ], - [ - "0.0", - "2318.2676880118993", - "462.7245851119162" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["2340.2520648903665", "0.0", "698.4611375636877"], + ["0.0", "2318.2676880118993", "462.7245851119162"], + ["0.0", "0.0", "1.0"] ], "kCamera2ComparisonGain": "0.8", "kCamera2Contrast": "1.2", @@ -321,16 +255,8 @@ "kCamera2FocalLength": "5.903208539", "kCamera2Gain": "6.0", "kCamera2Saturation": "1", - "kCamera2OffsetFromCamera1OriginMeters": [ - "0.00", - "-0.19", - "0.0" - ], - "kCamera2PositionsFromExpectedBallMeters": [ - "0.0", - "-0.051", - "0.45" - ], + "kCamera2OffsetFromCamera1OriginMeters": ["0.00", "-0.19", "0.0"], + "kCamera2PositionsFromExpectedBallMeters": ["0.0", "-0.051", "0.45"], "kCamera2PuttingContrast": "1.2", "kCamera2PuttingGain": "4.0", "kCamera2StillShutterTimeuS": "15000", @@ -355,6 +281,10 @@ "GSPro": { "kGSProConnectPort": "921" }, + "OpenGolfSim": { + "kOGSConnectAddress": "", + "kOGSConnectPort": "3111" + }, "kLaunchMonitorIdString": "PiTrac LM 0.1", "kSkipSpinCalculation": "0", "kWriteSpinAnalysisCsvFiles": "1" @@ -412,24 +342,10 @@ "kCam2SetupPeriodMilliseconds": "2000", "kConnectionBoardVersion": "2", "kEnclosureVersion": "2", - "kDynamicFollowOnPulseVectorPutter": [ - 444.0 - ], + "kDynamicFollowOnPulseVectorPutter": [444.0], "kLastPulsePutterRepeats": "0", "kLongerStrobePulseVectorDriver": [ - 0.175, - 0.7, - 1.4, - 2.45, - 1.26, - 2.8, - 2.1, - 3.15, - 3.85, - 3.85, - 10.4, - 3.5, - 0 + 0.175, 0.7, 1.4, 2.45, 1.26, 2.8, 2.1, 3.15, 3.85, 3.85, 10.4, 3.5, 0 ], "kNumberPrimingPulses": "12", "kPauseAfterSendingPreImageTriggerMs": "2000", @@ -443,31 +359,9 @@ "kPuttingBallSpeedSlowdownPercentage": "5.2", "kPuttingStrobeDelayMs": "50", "kStandardBallSpeedSlowdownPercentage": "0.1", - "kStrobePulseVectorDriver": [ - 0.7, - 1.8, - 3.0, - 2.2, - 3.0, - 7.1, - 4.0, - 4.0, - 0 - ], + "kStrobePulseVectorDriver": [0.7, 1.8, 3.0, 2.2, 3.0, 7.1, 4.0, 4.0, 0], "kStrobePulseVectorPutter": [ - 2.5, - 5.0, - 8.0, - 10.5, - 8.5, - 21.0, - 21.0, - 21.0, - 21.0, - 21.0, - 21.0, - 21.0, - 0 + 2.5, 5.0, 8.0, 10.5, 8.5, 21.0, 21.0, 21.0, 21.0, 21.0, 21.0, 21.0, 0 ], "number_bits_for_fast_on_pulse_": "2", "number_bits_for_slow_on_pulse_": "8" diff --git a/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.cpp b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.cpp new file mode 100644 index 00000000..79323ccc --- /dev/null +++ b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.cpp @@ -0,0 +1,209 @@ +#include +#include + +#include "gs_config.h" +#include "gs_options.h" +#include "gs_ui_system.h" +#include "logging_tools.h" + +#include "gs_opengolfsim_interface.h" +#include "gs_opengolfsim_results.h" + +namespace golf_sim { + +static const int kDefaultOGSPort = 3111; +static const char *kDefaultOGSHost = ""; // empty = disabled by default + +static std::string ToString(SimConnState s) { + switch (s) { + case SimConnState::kDisabled: + return "Disabled"; + case SimConnState::kDisconnected: + return "Disconnected"; + case SimConnState::kConnecting: + return "Connecting"; + case SimConnState::kConnected: + return "Connected"; + case SimConnState::kError: + return "Error"; + } + return "Unknown"; +} + +void GsOpenGolfSimInterface::OnConnectionStateChanged( + SimConnState /*from*/, SimConnState to, const std::string &reason) { + GsIPCResultType uiType = GsIPCResultType::kWaitingForSimulatorArmed; + std::string human; + + switch (to) { + case SimConnState::kDisabled: + human = "OpenGolfSim disabled (no host/port configured)"; + break; + + case SimConnState::kConnecting: + human = "OpenGolfSim connecting to " + socket_connect_address_ + ":" + + socket_connect_port_; + break; + + case SimConnState::kConnected: + break; + + case SimConnState::kDisconnected: + human = + "OpenGolfSim disconnected" + (reason.empty() ? "" : (": " + reason)); + break; + + case SimConnState::kError: + default: + uiType = GsIPCResultType::kError; + human = + "OpenGolfSim socket error" + (reason.empty() ? "" : (": " + reason)); + break; + } + + // machine tag (Python parses this) + std::string state = (to == SimConnState::kConnected) ? "connected" + : (to == SimConnState::kConnecting) ? "connecting" + : (to == SimConnState::kDisabled) ? "disabled" + : (to == SimConnState::kDisconnected) ? "disconnected" + : "error"; + + std::string tag = "SIM_CONN OpenGolfSim state=" + state; + if (!reason.empty()) + tag += " reason=" + reason; + + // ONE message: easy parse + still readable + std::string combined = "[" + tag + "] " + human; + + GsUISystem::SendIPCStatusMessage(uiType, combined); +} + +std::string GsOpenGolfSimInterface::EnsureNewline(const std::string &s) { + if (!s.empty() && s.back() == '\n') + return s; + return s + "\n"; +} + +GsOpenGolfSimInterface::GsOpenGolfSimInterface() { + // Defaults + socket_connect_address_ = kDefaultOGSHost; + socket_connect_port_ = + std::to_string(kDefaultOGSPort); // <-- port is a string in base class + last_device_status_.clear(); + + // Pull from config + GolfSimConfiguration::SetConstant( + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectAddress", + socket_connect_address_); + GolfSimConfiguration::SetConstant( + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectPort", + socket_connect_port_); +} + +GsOpenGolfSimInterface::~GsOpenGolfSimInterface() {} + +bool GsOpenGolfSimInterface::InterfaceIsPresent() { + // Read config into locals (static function!) + std::string addr = kDefaultOGSHost; + std::string port_str = std::to_string(kDefaultOGSPort); + + GolfSimConfiguration::SetConstant( + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectAddress", + addr); + GolfSimConfiguration::SetConstant( + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectPort", + port_str); + + int port = -1; + try { + port = std::stoi(port_str); + } catch (...) { + port = -1; + } + + // Treat empty/"disabled" as off + if (addr.empty() || addr == "disabled" || port <= 0) { + GS_LOG_TRACE_MSG( + trace, "GsOpenGolfSimInterface::InterfaceIsPresent - Not Present"); + return false; + } + + GS_LOG_TRACE_MSG( + trace, "GsOpenGolfSimInterface::InterfaceIsPresent - Present (addr=" + + addr + ", port=" + std::to_string(port) + ")"); + return true; +} + +bool GsOpenGolfSimInterface::Initialize() { + GS_LOG_TRACE_MSG(trace, "GsOpenGolfSimInterface Initialize called."); + + // Log what we will actually try to connect to + GS_LOG_MSG(info, "OpenGolfSim connect target: " + socket_connect_address_ + + ":" + socket_connect_port_); + + if (!GsSimSocketInterface::Initialize()) { + GS_LOG_MSG(error, "GsOpenGolfSimInterface could not Initialize."); + return false; + } + +#ifdef __unix__ + usleep(500); +#endif + + initialized_ = true; + + // Always ready on connect + SendSimMessage(EnsureNewline(R"({"type":"device","status":"ready"})")); + last_device_status_ = "ready"; // avoid immediate duplicate heartbeat + return true; +} + +void GsOpenGolfSimInterface::DeInitialize() { + GsSimSocketInterface::DeInitialize(); +} + +void GsOpenGolfSimInterface::SetSimSystemArmed(const bool /*is_armed*/) {} + +bool GsOpenGolfSimInterface::GetSimSystemArmed() { return true; } + +bool GsOpenGolfSimInterface::SendResults(const GsResults &r) { + if (!initialized_) + return false; + + // Heartbeat -> device status + if (r.result_message_is_keepalive_) { + const char *status = r.heartbeat_ball_detected_ ? "ready" : "busy"; + if (last_device_status_ == status) + return true; + last_device_status_ = status; + + std::string msg = + std::string(R"({"type":"device","status":")") + status + R"("})"; + SendSimMessage(EnsureNewline(msg)); + return true; + } + + // Normal shot path + const std::string msg = GenerateResultsDataToSend(r); + if (msg.empty()) + return true; + SendSimMessage(msg); + return true; +} + +std::string +GsOpenGolfSimInterface::GenerateResultsDataToSend(const GsResults &r) { + GsOpenGolfSimResults ogs(r); + const std::string payload = ogs.Format(); + if (payload.empty()) + return ""; + return EnsureNewline(payload); +} + +bool GsOpenGolfSimInterface::ProcessReceivedData( + const std::string received_data) { + GS_LOG_MSG(info, "Received from OpenGolfSim: " + received_data); + return true; +} + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.h b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.h new file mode 100644 index 00000000..aeccd726 --- /dev/null +++ b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_interface.h @@ -0,0 +1,36 @@ +// gs_opengolfsim_interface.h + +#pragma once +#include "gs_results.h" +#include "gs_sim_socket_interface.h" +#include + +namespace golf_sim { + +class GsOpenGolfSimInterface : public GsSimSocketInterface { +public: + GsOpenGolfSimInterface(); + virtual ~GsOpenGolfSimInterface(); + + static bool InterfaceIsPresent(); + + virtual bool Initialize() override; + virtual void DeInitialize() override; + + virtual bool SendResults(const GsResults &results) override; + + virtual void SetSimSystemArmed(const bool is_armed) override; + virtual bool GetSimSystemArmed() override; + +protected: + std::string EnsureNewline(const std::string &s); + virtual std::string GenerateResultsDataToSend(const GsResults &r); + virtual bool ProcessReceivedData(const std::string received_data) override; + void OnConnectionStateChanged(SimConnState from, SimConnState to, + const std::string &reason) override; + +private: + std::string last_device_status_; +}; + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.cpp b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.cpp new file mode 100644 index 00000000..dadf180e --- /dev/null +++ b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.cpp @@ -0,0 +1,51 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include +#include +#include + +#include "gs_opengolfsim_results.h" + +namespace golf_sim { + +std::string GsOpenGolfSimResults::Format() const { + // Don't ever send keepalives as shots + if (result_message_is_keepalive_) + return ""; + + // Basic sanity: ignore false triggers / mis-detections with near-zero speed + if (!std::isfinite(speed_mph_) || speed_mph_ < 3.0) + return ""; + + // Compute total spin magnitude from components + const double backSpinRpm = back_spin_rpm_; + const double sideSpinRpm = side_spin_rpm_; + const double spinSpeedRpm = + std::sqrt(backSpinRpm * backSpinRpm + sideSpinRpm * sideSpinRpm); + + // "20% above normal high wedge spin" filter (12,000 * 1.2 = 14,400) + constexpr double kMaxSpinRpm = 14400.0; + if (!std::isfinite(spinSpeedRpm) || spinSpeedRpm > kMaxSpinRpm) + return ""; + + // Map PiTrac fields to OpenGolfSim fields + const double ballSpeedMph = speed_mph_; + const double vLaunchDeg = vla_deg_; + const double hLaunchDeg = hla_deg_; + const double spinAxisDeg = GetSpinAxis(); + + // Optional: extra sanity on angles/axis + if (!std::isfinite(vLaunchDeg) || !std::isfinite(hLaunchDeg) || + !std::isfinite(spinAxisDeg)) + return ""; + + char buf[512]; + std::snprintf( + buf, sizeof(buf), + R"({"type":"shot","shot":{"ballSpeed":%.3f,"verticalLaunchAngle":%.3f,"horizontalLaunchAngle":%.3f,"spinAxis":%.3f,"spinSpeed":%.3f}})", + ballSpeedMph, vLaunchDeg, hLaunchDeg, spinAxisDeg, spinSpeedRpm); + + return std::string(buf); +} + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.h b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.h new file mode 100644 index 00000000..94743b26 --- /dev/null +++ b/Software/LMSourceCode/ImageProcessing/gs_opengolfsim_results.h @@ -0,0 +1,16 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +#pragma once + +#include "gs_results.h" + +namespace golf_sim { + +class GsOpenGolfSimResults : public GsResults { +public: + GsOpenGolfSimResults() = default; + explicit GsOpenGolfSimResults(const GsResults &results) + : GsResults(results) {} + std::string Format() const; +}; + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_sim_interface.cpp b/Software/LMSourceCode/ImageProcessing/gs_sim_interface.cpp index c1250bb6..9afa92c2 100644 --- a/Software/LMSourceCode/ImageProcessing/gs_sim_interface.cpp +++ b/Software/LMSourceCode/ImageProcessing/gs_sim_interface.cpp @@ -3,287 +3,315 @@ * Copyright (C) 2022-2025, Verdant Consultants, LLC. */ -#include "logging_tools.h" #include "cv_utils.h" -#include "gs_options.h" #include "gs_config.h" +#include "gs_options.h" +#include "logging_tools.h" -#include "gs_sim_interface.h" -#include "gs_gspro_interface.h" #include "gs_e6_interface.h" +#include "gs_gspro_interface.h" +#include "gs_opengolfsim_interface.h" +#include "gs_sim_interface.h" namespace golf_sim { - /* TBD - REMOVE - No longer static - bool GsSimInterface::sim_system_is_armed_ = false; - boost::mutex GsSimInterface::sim_arming_mutex_; - */ - std::string GsSimInterface::launch_monitor_id_string_ = "PiTrac LM 0.1"; +/* TBD - REMOVE - No longer static +bool GsSimInterface::sim_system_is_armed_ = false; +boost::mutex GsSimInterface::sim_arming_mutex_; +*/ +std::string GsSimInterface::launch_monitor_id_string_ = "PiTrac LM 0.1"; - std::vector GsSimInterface::interfaces_; - bool GsSimInterface::sims_initialized_ = false; +std::vector GsSimInterface::interfaces_; +bool GsSimInterface::sims_initialized_ = false; - // The first shot number the golf simulator receives should be 1, not 0, and - // the system will increment the counter first before storing information - long GsSimInterface::shot_counter_ = 0; +// The first shot number the golf simulator receives should be 1, not 0, and +// the system will increment the counter first before storing information +long GsSimInterface::shot_counter_ = 0; +GsSimInterface::GsSimInterface() { + GolfSimConfiguration::SetConstant( + "gs_config.golf_simulator_interfaces.kLaunchMonitorIdString", + launch_monitor_id_string_); +} - GsSimInterface::GsSimInterface() { - GolfSimConfiguration::SetConstant("gs_config.golf_simulator_interfaces.kLaunchMonitorIdString", launch_monitor_id_string_); - } - - GsSimInterface::~GsSimInterface() { - } - - bool GsSimInterface::InitializeSims() { - - GS_LOG_TRACE_MSG(trace, "GsSimInterface::InitializeSims()"); +GsSimInterface::~GsSimInterface() {} - // Create and add an interface to the global vector of interfaces - // for each configured sim +bool GsSimInterface::InitializeSims() { -#ifdef __unix__ // Ignore in Windows environment + GS_LOG_TRACE_MSG(trace, "GsSimInterface::InitializeSims()"); - if (GsGSProInterface::InterfaceIsPresent()) { - GS_LOG_TRACE_MSG(trace, "GSPro simulator interface detected."); + // Create and add an interface to the global vector of interfaces + // for each configured sim - GsGSProInterface* gspro_sim = new GsGSProInterface(); - if (gspro_sim == nullptr) { - GS_LOG_MSG(error, "Could not create a GSPro simulator interface."); - return false; - } +#ifdef __unix__ // Ignore in Windows environment - gspro_sim->simulator_type_ = GolfSimulatorType::kGSPro; + if (GsGSProInterface::InterfaceIsPresent()) { + GS_LOG_TRACE_MSG(trace, "GSPro simulator interface detected."); - interfaces_.push_back(gspro_sim); + GsGSProInterface *gspro_sim = new GsGSProInterface(); + if (gspro_sim == nullptr) { + GS_LOG_MSG(error, "Could not create a GSPro simulator interface."); + return false; + } - if (!gspro_sim->Initialize()) { - GS_LOG_MSG(error, "GSPro simulator interface could not be initialized."); - return false; - } - } + gspro_sim->simulator_type_ = GolfSimulatorType::kGSPro; - if (GsE6Interface::InterfaceIsPresent()) { - GS_LOG_TRACE_MSG(trace, "E6 simulator interface detected."); - GsE6Interface* e6_sim = new GsE6Interface(); - if (e6_sim == nullptr) { - GS_LOG_MSG(error, "Could not create an E6 simulator interface."); - return false; - } + interfaces_.push_back(gspro_sim); - e6_sim->simulator_type_ = GolfSimulatorType::kE6; + if (!gspro_sim->Initialize()) { + GS_LOG_MSG(error, "GSPro simulator interface could not be initialized."); + return false; + } + } + + if (GsE6Interface::InterfaceIsPresent()) { + GS_LOG_TRACE_MSG(trace, "E6 simulator interface detected."); + GsE6Interface *e6_sim = new GsE6Interface(); + if (e6_sim == nullptr) { + GS_LOG_MSG(error, "Could not create an E6 simulator interface."); + return false; + } - interfaces_.push_back(e6_sim); + e6_sim->simulator_type_ = GolfSimulatorType::kE6; - if (!e6_sim->Initialize()) { - GS_LOG_MSG(error, "E6 simulator interface could not be initialized."); - return false; - } - } + interfaces_.push_back(e6_sim); - if (interfaces_.size() == 0) { - GS_LOG_TRACE_MSG(trace, "No simulator interface detected."); - } -#endif - shot_counter_ = 0; + if (!e6_sim->Initialize()) { + GS_LOG_MSG(error, "E6 simulator interface could not be initialized."); + return false; + } + } - sims_initialized_ = true; + if (GsOpenGolfSimInterface::InterfaceIsPresent()) { + GS_LOG_TRACE_MSG(trace, "OpenGolfSim simulator interface detected."); - return true; + GsOpenGolfSimInterface *ogs_sim = new GsOpenGolfSimInterface(); + if (ogs_sim == nullptr) { + GS_LOG_MSG(error, "Could not create an OpenGolfSim simulator interface."); + return false; } + // You’ll probably want to add this enum value (see note below) + ogs_sim->simulator_type_ = GolfSimulatorType::kOpenGolfSim; - bool GsSimInterface::SimIsConnected() { + interfaces_.push_back(ogs_sim); - GS_LOG_TRACE_MSG(trace, "GsSimInterface::SimIsConnected()"); + if (!ogs_sim->Initialize()) { + GS_LOG_MSG(error, + "OpenGolfSim simulator interface could not be initialized."); + return false; + } + } - if (!sims_initialized_) { - // If we're not even initialized, there can't be any connected golf sims. - return false; - } + if (interfaces_.size() == 0) { + GS_LOG_TRACE_MSG(trace, "No simulator interface detected."); + } +#endif + shot_counter_ = 0; - bool sim_is_connected = false; + sims_initialized_ = true; -#ifdef __unix__ // Ignore in Windows environment + return true; +} - for (auto interface : interfaces_) { - if (interface != nullptr) { - sim_is_connected = true; - continue; - } - } -#endif - return sim_is_connected; - } +bool GsSimInterface::SimIsConnected() { - void GsSimInterface::DeInitializeSims() { + GS_LOG_TRACE_MSG(trace, "GsSimInterface::SimIsConnected()"); - GS_LOG_TRACE_MSG(trace, "GsSimInterface::DeInitializeSims()"); + if (!sims_initialized_) { + // If we're not even initialized, there can't be any connected golf sims. + return false; + } -#ifdef __unix__ // Ignore in Windows environment + bool sim_is_connected = false; - for (auto interface : interfaces_) { - if (interface == nullptr) { - GS_LOG_MSG(error, "GsSimInterface::DeInitializeSims() found a null interface"); - continue; - } +#ifdef __unix__ // Ignore in Windows environment - interface->DeInitialize(); - delete interface; - } -#endif - sims_initialized_ = false; + for (auto interface : interfaces_) { + if (interface != nullptr) { + sim_is_connected = true; + continue; } + } +#endif + return sim_is_connected; +} +void GsSimInterface::DeInitializeSims() { - void GsSimInterface::SetSimSystemArmed(const bool is_armed) { - boost::lock_guard lock(sim_arming_mutex_); + GS_LOG_TRACE_MSG(trace, "GsSimInterface::DeInitializeSims()"); - // At the generic level, we'll just allow for setting and getting - // the system-armed status even though this is a virtual interface - GS_LOG_TRACE_MSG(trace, "GsSimInterface::SetSimSystemArmed called."); +#ifdef __unix__ // Ignore in Windows environment - sim_system_is_armed_ = is_armed; - } + for (auto interface : interfaces_) { + if (interface == nullptr) { + GS_LOG_MSG(error, + "GsSimInterface::DeInitializeSims() found a null interface"); + continue; + } - bool GsSimInterface::GetSimSystemArmed() { - boost::lock_guard lock(sim_arming_mutex_); + interface->DeInitialize(); + delete interface; + } +#endif + sims_initialized_ = false; +} - if (!SimIsConnected()) { - // If no sims, then consider the system armed as a reasonable fallback behavior - return true; - } +void GsSimInterface::SetSimSystemArmed(const bool is_armed) { + boost::lock_guard lock(sim_arming_mutex_); - return sim_system_is_armed_; - } + // At the generic level, we'll just allow for setting and getting + // the system-armed status even though this is a virtual interface + GS_LOG_TRACE_MSG(trace, "GsSimInterface::SetSimSystemArmed called."); + sim_system_is_armed_ = is_armed; +} - int GsSimInterface::SendSimMessage(const std::string& message) { - GS_LOG_MSG(warning, "GsSimInterface::SendSimMessage - message was:\n" + message); - return 0; - } +bool GsSimInterface::GetSimSystemArmed() { + boost::lock_guard lock(sim_arming_mutex_); + if (!SimIsConnected()) { + // If no sims, then consider the system armed as a reasonable fallback + // behavior + return true; + } - bool GsSimInterface::SendResultsToGolfSims(const GsResults& input_results) { + return sim_system_is_armed_; +} - // The shot number should already have been set when the ball was teed up +int GsSimInterface::SendSimMessage(const std::string &message) { + GS_LOG_MSG(warning, + "GsSimInterface::SendSimMessage - message was:\n" + message); + return 0; +} - // Make a local copy of the results so that we can set the shot_counter - GsResults results = input_results; - results.shot_number_ = shot_counter_; +bool GsSimInterface::SendResultsToGolfSims(const GsResults &input_results) { - if (results.speed_mph_ > 200.0) { - GS_LOG_MSG(warning, "GsSimInterface::SendResultsToGolfSim got out of bounds speed_mph. Settting to 200."); - results.speed_mph_ = 200.0; - } + // The shot number should already have been set when the ball was teed up - bool status = true; + // Make a local copy of the results so that we can set the shot_counter + GsResults results = input_results; + results.shot_number_ = shot_counter_; -#ifdef __unix__ // Ignore in Windows environment + if (results.speed_mph_ > 200.0) { + GS_LOG_MSG(warning, "GsSimInterface::SendResultsToGolfSim got out of " + "bounds speed_mph. Settting to 200."); + results.speed_mph_ = 200.0; + } - // Loop through any interfaces that we are configured for and send the results - for (auto interface : interfaces_) { - if (interface == nullptr) { - GS_LOG_MSG(error, "GsSimInterface::DeInitializeSims() found a null interface"); - continue; - } + bool status = true; - interface->SendResults(results); - } +#ifdef __unix__ // Ignore in Windows environment -#endif - return status; + // Loop through any interfaces that we are configured for and send the results + for (auto interface : interfaces_) { + if (interface == nullptr) { + GS_LOG_MSG(error, + "GsSimInterface::DeInitializeSims() found a null interface"); + continue; } - bool GsSimInterface::GetAllSystemsArmed() { - bool all_systems_armed = true; + interface->SendResults(results); + } - // Loop through any interfaces that we are configured for and send the results - for (auto interface : interfaces_) { - if (interface == nullptr) { - GS_LOG_MSG(error, "GsSimInterface::DeInitializeSims() found a null interface"); - continue; - } +#endif + return status; +} - // If even one interface is not armed, then we're not "all" ready - if (!interface->GetSimSystemArmed()) { - all_systems_armed = false; - } - } +bool GsSimInterface::GetAllSystemsArmed() { + bool all_systems_armed = true; - return all_systems_armed; + // Loop through any interfaces that we are configured for and send the results + for (auto interface : interfaces_) { + if (interface == nullptr) { + GS_LOG_MSG(error, + "GsSimInterface::DeInitializeSims() found a null interface"); + continue; } + // If even one interface is not armed, then we're not "all" ready + if (!interface->GetSimSystemArmed()) { + all_systems_armed = false; + } + } - GsSimInterface* GsSimInterface::GetSimInterfaceByType(GolfSimulatorType sim_type) { + return all_systems_armed; +} - // Loop through any interfaces that we are configured for and send the results - for (auto interface : interfaces_) { - if (interface == nullptr) { - GS_LOG_MSG(error, "GsSimInterface::DeInitializeSims() found a null interface"); - continue; - } +GsSimInterface * +GsSimInterface::GetSimInterfaceByType(GolfSimulatorType sim_type) { - if (interface->simulator_type_ == sim_type) { - return interface; - } - } + // Loop through any interfaces that we are configured for and send the results + for (auto interface : interfaces_) { + if (interface == nullptr) { + GS_LOG_MSG(error, + "GsSimInterface::DeInitializeSims() found a null interface"); + continue; + } - return nullptr; + if (interface->simulator_type_ == sim_type) { + return interface; } + } + return nullptr; +} - void GsSimInterface::IncrementShotCounter() { - shot_counter_++; - } +void GsSimInterface::IncrementShotCounter() { shot_counter_++; } - void GsSimInterface::SendHeartbeat(bool ball_detected) { -#ifdef __unix__ // Ignore in Windows environment - if (!sims_initialized_) { - return; - } - - GsResults heartbeat; - heartbeat.result_message_is_keepalive_ = true; - heartbeat.heartbeat_ball_detected_ = ball_detected; - heartbeat.heartbeat_launch_monitor_ready_ = true; - - for (auto interface : interfaces_) { - if (interface == nullptr) { - continue; - } - interface->SendResults(heartbeat); - } -#endif - } +void GsSimInterface::SendHeartbeat(bool ball_detected) { +#ifdef __unix__ // Ignore in Windows environment + if (!sims_initialized_) { + return; + } - bool GsSimInterface::InterfaceIsPresent() { - // The base interface isn't a real interface, so cannot be 'present' - return false; - } + GsResults heartbeat; + heartbeat.result_message_is_keepalive_ = true; + heartbeat.heartbeat_ball_detected_ = ball_detected; + heartbeat.heartbeat_launch_monitor_ready_ = true; - bool GsSimInterface::Initialize() { - // The base interface isn't a real interface, so cannot be initialized - return false; + for (auto interface : interfaces_) { + if (interface == nullptr) { + continue; } + interface->SendResults(heartbeat); + } +#endif +} - void GsSimInterface::DeInitialize() { - // The base interface isn't a real interface, so cannot be de-initializeds - return; - } +bool GsSimInterface::InterfaceIsPresent() { + // The base interface isn't a real interface, so cannot be 'present' + return false; +} - bool GsSimInterface::SendResults(const GsResults& results) { - GS_LOG_TRACE_MSG(trace, "GsSimInterface::SendResults - No Golf Sim connected to Launch Monitor. Results are: " + results.Format()); - return true; - } +bool GsSimInterface::Initialize() { + // The base interface isn't a real interface, so cannot be initialized + return false; +} - std::string GsSimInterface::GenerateResultsDataToSend(const GsResults& results) { - return results.Format(); - } +void GsSimInterface::DeInitialize() { + // The base interface isn't a real interface, so cannot be de-initializeds + return; +} - bool GsSimInterface::ProcessReceivedData(const std::string received_data) { - GS_LOG_TRACE_MSG(trace, "GsSimInterface::ProcessReceivedData - No Golf Sim connected to Launch Monitor, so not doing anything with data. Data was:\n" + received_data); - return true; - } +bool GsSimInterface::SendResults(const GsResults &results) { + GS_LOG_TRACE_MSG(trace, "GsSimInterface::SendResults - No Golf Sim connected " + "to Launch Monitor. Results are: " + + results.Format()); + return true; +} +std::string +GsSimInterface::GenerateResultsDataToSend(const GsResults &results) { + return results.Format(); } + +bool GsSimInterface::ProcessReceivedData(const std::string received_data) { + GS_LOG_TRACE_MSG( + trace, "GsSimInterface::ProcessReceivedData - No Golf Sim connected to " + "Launch Monitor, so not doing anything with data. Data was:\n" + + received_data); + return true; +} + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_sim_interface.h b/Software/LMSourceCode/ImageProcessing/gs_sim_interface.h index 7e12e237..7f59184d 100644 --- a/Software/LMSourceCode/ImageProcessing/gs_sim_interface.h +++ b/Software/LMSourceCode/ImageProcessing/gs_sim_interface.h @@ -8,114 +8,106 @@ #include #include - -#include "logging_tools.h" #include "golf_ball.h" #include "gs_results.h" - +#include "logging_tools.h" // Base class for interfaces to 3rd-party golf simulators namespace golf_sim { - class GsSimInterface { - - public: - enum GolfSimulatorType { - kNone = 0, - kGSPro = 1, - kE6 = 2 - }; +class GsSimInterface { - GsSimInterface(); - virtual ~GsSimInterface(); +public: + enum GolfSimulatorType { kNone = 0, kGSPro = 1, kE6 = 2, kOpenGolfSim = 3 }; - // Create and initialize and sim interfaces that are configured - static bool InitializeSims(); + GsSimInterface(); + virtual ~GsSimInterface(); - // De-initialize and destory and sim interfaces that are configured - static void DeInitializeSims(); + // Create and initialize and sim interfaces that are configured + static bool InitializeSims(); - // Returns true if at least one golf sim is connected to the system. - static bool SimIsConnected(); + // De-initialize and destory and sim interfaces that are configured + static void DeInitializeSims(); - // To be called from the launch monitor - static bool SendResultsToGolfSims(const GsResults& results); + // Returns true if at least one golf sim is connected to the system. + static bool SimIsConnected(); - // If the interface is present (usually indicated in the config.json file), - // this method returns true; - static bool InterfaceIsPresent(); + // To be called from the launch monitor + static bool SendResultsToGolfSims(const GsResults &results); - // Allows the shot counter to be incremented from outside the simulator - // interface for such purposes and ensuring the counter keeps going even - // when a failure occurs. + // If the interface is present (usually indicated in the config.json file), + // this method returns true; + static bool InterfaceIsPresent(); - static void IncrementShotCounter(); + // Allows the shot counter to be incremented from outside the simulator + // interface for such purposes and ensuring the counter keeps going even + // when a failure occurs. - // Will be overridden by each derived class + static void IncrementShotCounter(); - virtual bool Initialize(); + // Will be overridden by each derived class - // De-initialize and destroy and sim interfaces that are configured - virtual void DeInitialize(); + virtual bool Initialize(); - // Base class behavior is to simply print out the JSON - virtual bool SendResults(const GsResults& results); + // De-initialize and destroy and sim interfaces that are configured + virtual void DeInitialize(); - // Sends a string without any other side-effects - // Returns the number of bytes written - virtual int SendSimMessage(const std::string& message); + // Base class behavior is to simply print out the JSON + virtual bool SendResults(const GsResults &results); - // Deals with whether or not ALL of the connected simulators are armed - // (ready to take a shot). Some sims just return true. - virtual void SetSimSystemArmed(const bool is_armed); - virtual bool GetSimSystemArmed(); + // Sends a string without any other side-effects + // Returns the number of bytes written + virtual int SendSimMessage(const std::string &message); - // These static functions operate at the collection level for all interfaces - static long GetShotCounter() { return shot_counter_; }; + // Deals with whether or not ALL of the connected simulators are armed + // (ready to take a shot). Some sims just return true. + virtual void SetSimSystemArmed(const bool is_armed); + virtual bool GetSimSystemArmed(); - // Find the GSPro or E6 or whatever interface (if available) by type - static GsSimInterface *GetSimInterfaceByType(GolfSimulatorType sim_type); + // These static functions operate at the collection level for all interfaces + static long GetShotCounter() { return shot_counter_; }; - // Returns true only if each of the available interfaces is armed - static bool GetAllSystemsArmed(); + // Find the GSPro or E6 or whatever interface (if available) by type + static GsSimInterface *GetSimInterfaceByType(GolfSimulatorType sim_type); - // Heartbeat support for external simulators - static void SendHeartbeat(bool ball_detected); - static inline void ResetHeartbeatState() {} + // Returns true only if each of the available interfaces is armed + static bool GetAllSystemsArmed(); - protected: + // Heartbeat support for external simulators + static void SendHeartbeat(bool ball_detected); + static inline void ResetHeartbeatState() {} - // Typical derived-class behavior will be to convert the results into a - // sim-specific data packet, such as a JSON string - virtual std::string GenerateResultsDataToSend(const GsResults& results); +protected: + // Typical derived-class behavior will be to convert the results into a + // sim-specific data packet, such as a JSON string + virtual std::string GenerateResultsDataToSend(const GsResults &results); - // Called when the LM receives data - virtual bool ProcessReceivedData(const std::string received_data); + // Called when the LM receives data + virtual bool ProcessReceivedData(const std::string received_data); - protected: +protected: + // Holds pointers to derived interfaces for each attached sim + static std::vector interfaces_; - // Holds pointers to derived interfaces for each attached sim - static std::vector interfaces_; + static std::string launch_monitor_id_string_; - static std::string launch_monitor_id_string_; - - // True if all the attached sims have been initialized - static bool sims_initialized_; + // True if all the attached sims have been initialized + static bool sims_initialized_; - static long shot_counter_; + static long shot_counter_; - // True if all THIS sim has been initialized - bool initialized_; + // True if all THIS sim has been initialized + bool initialized_; - GolfSimulatorType simulator_type_; + GolfSimulatorType simulator_type_; - // Must be true before the simulator system is ready to accept shot data - // Only relevant for derived, non-virtual classes for whom arming is an - // actual thing. - bool sim_system_is_armed_ = false; + // Must be true before the simulator system is ready to accept shot data + // Only relevant for derived, non-virtual classes for whom arming is an + // actual thing. + bool sim_system_is_armed_ = false; - boost::mutex sim_arming_mutex_; - }; + boost::mutex sim_arming_mutex_; +}; -} +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.cpp b/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.cpp index 5e960c1a..cf2f7600 100644 --- a/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.cpp +++ b/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.cpp @@ -5,279 +5,335 @@ #include -#ifdef __unix__ // Ignore in Windows environment +#ifdef __unix__ // Ignore in Windows environment -#include #include #include #include +#include -#include "logging_tools.h" -#include "gs_options.h" #include "gs_config.h" #include "gs_events.h" #include "gs_ipc_control_msg.h" +#include "gs_options.h" +#include "logging_tools.h" -#include "gs_sim_socket_interface.h" #include "gs_gspro_interface.h" #include "gs_gspro_response.h" #include "gs_gspro_results.h" +#include "gs_sim_socket_interface.h" using namespace boost::asio; using ip::tcp; - namespace golf_sim { - GsSimSocketInterface::GsSimSocketInterface() { - } +GsSimSocketInterface::GsSimSocketInterface() {} - GsSimSocketInterface::~GsSimSocketInterface() { +GsSimSocketInterface::~GsSimSocketInterface() {} - } +bool GsSimSocketInterface::InterfaceIsPresent() { + // The socket interface is basically just a base class, so cannot on it's own + // ber present + GS_LOG_TRACE_MSG( + trace, + "GsSimSocketInterface InterfaceIsPresent should not have been called."); + return false; +} - bool GsSimSocketInterface::InterfaceIsPresent() { - // The socket interface is basically just a base class, so cannot on it's own ber present - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface InterfaceIsPresent should not have been called."); - return false; +bool GsSimSocketInterface::Initialize() { + + // Derived classes must set the socket connection address and port before + // calling this function + + // Setup the socket connect here first so + // that we don't have to repeatedly do so. May also want to + // setup a keep-alive ping to the SimSocket system. + GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface Initialize called."); + + // Treat empty/disabled config as disabled + if (socket_connect_address_.empty() || + socket_connect_address_ == "disabled" || socket_connect_port_.empty() || + socket_connect_port_ == "0") { + SetConnectionState(SimConnState::kDisabled, "No sim host/port configured"); + GS_LOG_MSG(info, "Sim socket disabled (no host/port configured)."); + return false; // InterfaceIsPresent() should prevent this being called + // anyway + } + + SetConnectionState(SimConnState::kConnecting); + GS_LOG_MSG(info, "Connecting to SimSocketServer at: " + + socket_connect_address_ + ":" + socket_connect_port_); + + try { + io_context_ = new boost::asio::io_context(); + + if (io_context_ == nullptr) { + GS_LOG_MSG(error, + "GsSimSocketInterface could not create a new io_context."); + return false; } - bool GsSimSocketInterface::Initialize() { - - // Derived classes must set the socket connection address and port before calling this function - - // Setup the socket connect here first so - // that we don't have to repeatedly do so. May also want to - // setup a keep-alive ping to the SimSocket system. - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface Initialize called."); + tcp::resolver resolver(*io_context_); + GS_LOG_TRACE_MSG(trace, "Connecting to SimSocketServer at address: " + + socket_connect_address_ + ":" + + socket_connect_port_); + tcp::resolver::results_type endpoints = + resolver.resolve(socket_connect_address_, socket_connect_port_); - try - { - io_context_ = new boost::asio::io_context(); + // Create the socket if we haven't done so already + if (socket_ == nullptr) { + socket_ = new tcp::socket(*io_context_); - if (io_context_ == nullptr) { - GS_LOG_MSG(error, "GsSimSocketInterface could not create a new io_context."); - return false; - } + if (socket_ == nullptr) { + GS_LOG_MSG(error, + "GsSimSocketInterface could not create a new socket."); + return false; + } + } - tcp::resolver resolver(*io_context_); - GS_LOG_TRACE_MSG(trace, "Connecting to SimSocketServer at address: " + socket_connect_address_ + ":" + socket_connect_port_); - tcp::resolver::results_type endpoints = resolver.resolve(socket_connect_address_, socket_connect_port_); + boost::asio::connect(*socket_, endpoints); - // Create the socket if we haven't done so already - if (socket_ == nullptr) { - socket_ = new tcp::socket(*io_context_); + SetConnectionState(SimConnState::kConnected); + GS_LOG_MSG(info, "Connected to SimSocketServer at: " + + socket_connect_address_ + ":" + socket_connect_port_); - if (socket_ == nullptr) { - GS_LOG_MSG(error, "GsSimSocketInterface could not create a new socket."); - return false; - } - } + receiver_thread_ = std::unique_ptr( + new std::thread(&GsSimSocketInterface::ReceiveSocketData, this)); + // GS_LOG_TRACE_MSG(trace, "Thread was created. Thread id: " + + // std::string(receiver_thread_.get()->get_id()) ); - boost::asio::connect(*socket_, endpoints); + // socket_->set_option(boost::asio::detail::socket_option::integer{ 10 }); + } catch (std::exception &e) { + SetConnectionState(SimConnState::kError, e.what()); + GS_LOG_MSG(error, "Failed TestSimSocketMessage - Error was: " + + std::string(e.what())); + return false; + } - receiver_thread_ = std::unique_ptr(new std::thread(&GsSimSocketInterface::ReceiveSocketData, this)); +#ifdef __unix__ // Ignore in Windows environment + // Give the new thread a moment to get running + usleep(500); +#endif - // GS_LOG_TRACE_MSG(trace, "Thread was created. Thread id: " + std::string(receiver_thread_.get()->get_id()) ); + initialized_ = true; - // socket_->set_option(boost::asio::detail::socket_option::integer{ 10 }); - } - catch (std::exception& e) - { - GS_LOG_MSG(error, "Failed TestSimSocketMessage - Error was: " + std::string(e.what())); - return false; - } + // Connection just came up – make sure the first heartbeat reports no ball + // detected. + GsSimInterface::ResetHeartbeatState(); + GsSimInterface::SendHeartbeat(false); -#ifdef __unix__ // Ignore in Windows environment - // Give the new thread a moment to get running - usleep(500); -#endif + // Derived classes will need to deal with any initial messaging after the + // socket is established. - initialized_ = true; + return true; +} - // Connection just came up – make sure the first heartbeat reports no ball detected. - GsSimInterface::ResetHeartbeatState(); - GsSimInterface::SendHeartbeat(false); +void GsSimSocketInterface::ReceiveSocketData() { + receive_thread_exited_ = false; + + std::array buf{}; // 2000 bytes payload + null terminator + boost::system::error_code error; + std::string received_data_string; + + while (GolfSimGlobals::golf_sim_running_) { + GS_LOG_TRACE_MSG(trace, "Waiting to receive data from SimSocketserver."); + + size_t len = 0; + + try { + // IMPORTANT: read at most (size-1) so we can always null-terminate + len = socket_->read_some(boost::asio::buffer(buf.data(), buf.size() - 1), + error); + } catch (const std::exception &e) { + GS_LOG_MSG(error, "GsSimSocketInterface::ReceiveSocketData failed to " + "read from socket - Error was: " + + std::string(e.what())); + SetConnectionState(SimConnState::kError, e.what()); + receive_thread_exited_ = true; + return; + } - // Derived classes will need to deal with any initial messaging after the socket is established. + if (error == boost::asio::error::eof) { + GS_LOG_TRACE_MSG(trace, + "GsSimSocketInterface::ReceiveSocketData Received EOF"); + SetConnectionState(SimConnState::kDisconnected, "EOF"); + receive_thread_exited_ = true; + return; + } - return true; + if (error) { + GS_LOG_MSG(error, "Sim socket receive error: " + error.message()); + SetConnectionState(SimConnState::kError, error.message()); + receive_thread_exited_ = true; + return; } - void GsSimSocketInterface::ReceiveSocketData() { - - receive_thread_exited_ = false; - - static std::array buf; - boost::system::error_code error; - std::string received_data_string; - - while (GolfSimGlobals::golf_sim_running_) - { - // We don't want to re-enter this while we're processing - // a received message - // boost::lock_guard lock(sim_socket_receive_mutex_); - - GS_LOG_TRACE_MSG(trace, "Waiting to receive data from SimSocketserver."); - - size_t len = 0; - - try { - // Read_some should be blocking, but if the socket has closed, it will return immediately - len = socket_->read_some(boost::asio::buffer(buf), error); - } - catch (std::exception& e) - { - GS_LOG_MSG(error, "GsSimSocketInterface::ReceiveSocketData failed to read from socket - Error was: " + std::string(e.what())); - } - - if (len == 0) { - GS_LOG_MSG(warning, "Received 0-length message from server. Will attempt to re-initialize"); - /// TBD - Are we sure we want to exit? - receive_thread_exited_ = true; - return; - } - - // Null-terminate the string - buf[len] = (char)0; - received_data_string = std::string(buf.data()); - GS_LOG_TRACE_MSG(trace, " Read some data (" + std::to_string(len) + " bytes) : " + received_data_string); - - if (error == boost::asio::error::eof) { - // Connection closed cleanly by peer. - // In this case, we may want to de-initialize - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::ReceiveSocketData Received EOF"); - receive_thread_exited_ = true; - return; - } - else if (error) { - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::ReceiveSocketData Received Error"); - throw boost::system::system_error(error); // Some other error. - } - // Derived classes will, for example, parse the message and inject any - // relevant events into the FSM. - - GS_LOG_TRACE_MSG(trace, "Received SimSocket message of: \n" + received_data_string); - - if (!ProcessReceivedData(received_data_string)) { - GS_LOG_MSG(error, "Failed GsSimSocketInterface::ReceiveSocketData - Could process data: " + received_data_string); - return; - } - } - - GS_LOG_MSG(error, "GsSimSocketInterface::ReceiveSocketData Exiting"); + if (len == 0) { + GS_LOG_MSG(warning, "Received 0-length message from server."); + SetConnectionState(SimConnState::kDisconnected, "0-length read"); + receive_thread_exited_ = true; + return; } - void GsSimSocketInterface::DeInitialize() { + // Null-terminate and build string + buf[len] = '\0'; + received_data_string.assign(buf.data(), len); - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::DeInitialize() called."); - try { + GS_LOG_TRACE_MSG(trace, " Read some data (" + std::to_string(len) + + " bytes) : " + received_data_string); + GS_LOG_TRACE_MSG(trace, "Received SimSocket message of: \n" + + received_data_string); - if (receiver_thread_ != nullptr) { - /*** TBD - Was locking up - GS_LOG_TRACE_MSG(trace, "Waiting for join of receiver_thread_."); - receiver_thread_->join(); - receiver_thread_.release(); - delete receiver_thread_.get(); - */ - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::DeInitialize() killing receive thread."); + if (!ProcessReceivedData(received_data_string)) { + GS_LOG_MSG(error, "ProcessReceivedData failed"); + SetConnectionState(SimConnState::kError, "ProcessReceivedData failed"); + receive_thread_exited_ = true; + return; + } + } -#ifdef __unix__ // Ignore in Windows environment - pthread_cancel(receiver_thread_.get()->native_handle()); + GS_LOG_MSG(info, "GsSimSocketInterface::ReceiveSocketData Exiting"); +} + +void GsSimSocketInterface::DeInitialize() { + SetConnectionState(SimConnState::kDisconnected, "DeInitialize()"); + GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::DeInitialize() called."); + try { + + if (receiver_thread_ != nullptr) { + /*** TBD - Was locking up + GS_LOG_TRACE_MSG(trace, "Waiting for join of receiver_thread_."); + receiver_thread_->join(); + receiver_thread_.release(); + delete receiver_thread_.get(); + */ + GS_LOG_TRACE_MSG( + trace, + "GsSimSocketInterface::DeInitialize() killing receive thread."); + +#ifdef __unix__ // Ignore in Windows environment + pthread_cancel(receiver_thread_.get()->native_handle()); #endif - receiver_thread_ = nullptr; - } - - // TBD - not sure how to deinitialize the TCP socket stuff - delete socket_; - socket_ = nullptr; - delete io_context_; - io_context_ = nullptr; - - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::DeInitialize() completed."); - } - catch (std::exception& e) - { - GS_LOG_MSG(error, "Failed GsSimSocketInterface::DeInitialize() - Error was: " + std::string(e.what())); - } - - initialized_ = false; + receiver_thread_ = nullptr; } - int GsSimSocketInterface::SendSimMessage(const std::string& message) { - size_t write_length = 0; - boost::system::error_code error; - - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::SendSimMessage - Message was: " + message); + // TBD - not sure how to deinitialize the TCP socket stuff + delete socket_; + socket_ = nullptr; + delete io_context_; + io_context_ = nullptr; - // We don't want to re-enter this while we're processing - // a received message - boost::lock_guard lock(sim_socket_send_mutex_); + GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::DeInitialize() completed."); + } catch (std::exception &e) { + GS_LOG_MSG(error, + "Failed GsSimSocketInterface::DeInitialize() - Error was: " + + std::string(e.what())); + } + initialized_ = false; +} - try { +int GsSimSocketInterface::SendSimMessage(const std::string &message) { + size_t write_length = 0; + boost::system::error_code error; - write_length = socket_->write_some(boost::asio::buffer(message), error); - } - catch (std::exception& e) - { - GS_LOG_MSG(error, "Failed TestE6Message - Error was: " + std::string(e.what()) + ". Error code was:" + std::to_string(error.value()) ); - return -2; - } + if (!socket_) { + SetConnectionState(SimConnState::kDisconnected, + "socket_ was null on write"); + return -1; + } - return write_length; - } + GS_LOG_TRACE_MSG( + trace, "GsSimSocketInterface::SendSimMessage - Message was: " + message); + // We don't want to re-enter this while we're processing + // a received message + boost::lock_guard lock(sim_socket_send_mutex_); - bool GsSimSocketInterface::SendResults(const GsResults& results) { + try { - if (!initialized_) { - GS_LOG_MSG(error, "GsSimSocketInterface::SendResults called before the interface was intialized."); - return false; - } + write_length = socket_->write_some(boost::asio::buffer(message), error); - if (receive_thread_exited_) { - GS_LOG_MSG(error, "GsSimSocketInterface::SendResults called before the interface was intialized - trying to re-initialize."); - // If we ended the receive thread, try re-initializing the connection - DeInitialize(); - if (!Initialize()) { - GS_LOG_MSG(error, "GsSimSocketInterface::SendResults could not re-intialize thew interface."); - return false; - } - } + if (error) { + SetConnectionState(SimConnState::kError, error.message()); + receive_thread_exited_ = true; // forces reconnect path in SendResults + GS_LOG_MSG(error, "Sim socket write failed: " + error.message()); + return -2; + } + } catch (std::exception &e) { + SetConnectionState(SimConnState::kError, e.what()); + receive_thread_exited_ = true; + GS_LOG_MSG(error, + "Failed TestE6Message - Error was: " + std::string(e.what()) + + ". Error code was:" + std::to_string(error.value())); + return -2; + } + + return write_length; +} - GS_LOG_TRACE_MSG(trace, "Sending GsSimSocketInterface::SendResult results input message:\n" + results.Format()); +bool GsSimSocketInterface::SendResults(const GsResults &results) { + + if (!initialized_) { + GS_LOG_MSG(error, "GsSimSocketInterface::SendResults called before the " + "interface was intialized."); + return false; + } + + if (receive_thread_exited_) { + GS_LOG_MSG(error, "GsSimSocketInterface::SendResults called before the " + "interface was intialized - trying to re-initialize."); + // If we ended the receive thread, try re-initializing the connection + DeInitialize(); + if (!Initialize()) { + GS_LOG_MSG(error, "GsSimSocketInterface::SendResults could not " + "re-intialize thew interface."); + return false; + } + } - size_t write_length = -1; + GS_LOG_TRACE_MSG( + trace, + "Sending GsSimSocketInterface::SendResult results input message:\n" + + results.Format()); - try { - static std::array buf; - boost::system::error_code error; + size_t write_length = -1; - std::string results_msg = GenerateResultsDataToSend(results); + try { + static std::array buf; + boost::system::error_code error; - write_length = SendSimMessage(results_msg); - } - catch (std::exception& e) - { - GS_LOG_MSG(error, "Failed TestSimSocketMessage - Error was: " + std::string(e.what())); - return false; - } + std::string results_msg = GenerateResultsDataToSend(results); - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::SendResult sent " + std::to_string(write_length) + " bytes."); + write_length = SendSimMessage(results_msg); + } catch (std::exception &e) { + GS_LOG_MSG(error, "Failed TestSimSocketMessage - Error was: " + + std::string(e.what())); + return false; + } - return true; - } + GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::SendResult sent " + + std::to_string(write_length) + " bytes."); - std::string GsSimSocketInterface::GenerateResultsDataToSend(const GsResults& results) { - return results.Format(); - } + return true; +} - bool GsSimSocketInterface::ProcessReceivedData(const std::string received_data) { - GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::ProcessReceivedData - No Scoket-based Golf Sim connected to Launch Monitor, so not doing anything with data. Data was:\n" + received_data); - return true; - } +std::string +GsSimSocketInterface::GenerateResultsDataToSend(const GsResults &results) { + return results.Format(); +} +bool GsSimSocketInterface::ProcessReceivedData( + const std::string received_data) { + GS_LOG_TRACE_MSG(trace, "GsSimSocketInterface::ProcessReceivedData - No " + "Scoket-based Golf Sim connected to Launch Monitor, " + "so not doing anything with data. Data was:\n" + + received_data); + return true; } + +} // namespace golf_sim #endif diff --git a/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.h b/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.h index 3abbbcb5..b7715b74 100644 --- a/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.h +++ b/Software/LMSourceCode/ImageProcessing/gs_sim_socket_interface.h @@ -5,8 +5,12 @@ #pragma once +#include #include #include +#include +#include +#include #include "gs_results.h" #include "gs_sim_interface.h" @@ -18,52 +22,105 @@ using ip::tcp; namespace golf_sim { - class GsSimSocketInterface : public GsSimInterface { +enum class SimConnState { + kDisabled = 0, + kDisconnected, + kConnecting, + kConnected, + kError +}; - public: - GsSimSocketInterface(); - virtual ~GsSimSocketInterface(); +class GsSimSocketInterface : public GsSimInterface { - // Returns true iff the SimSocket interface is to be used - static bool InterfaceIsPresent(); +public: + GsSimSocketInterface(); + virtual ~GsSimSocketInterface(); - // Must be called before SendResults is called. - virtual bool Initialize(); + // Returns true iff the SimSocket interface is to be used + static bool InterfaceIsPresent(); - // Deals with, for example, shutting down any socket connection - virtual void DeInitialize(); + // Must be called before SendResults is called. + virtual bool Initialize(); - virtual bool SendResults(const GsResults& results); + // Deals with, for example, shutting down any socket connection + virtual void DeInitialize(); - virtual void ReceiveSocketData(); + virtual bool SendResults(const GsResults &results); - public: + virtual void ReceiveSocketData(); - std::string socket_connect_address_; - std::string socket_connect_port_; + // ---- connection state API ---- + SimConnState GetConnectionState() const { + return connection_state_.load(std::memory_order_relaxed); + } - protected: + bool IsConnected() const { + return GetConnectionState() == SimConnState::kConnected; + } - virtual std::string GenerateResultsDataToSend(const GsResults& results); - - virtual bool ProcessReceivedData(const std::string received_data); + std::string GetLastConnectionError() const { + boost::lock_guard lock(conn_state_mutex_); + return last_connection_error_; + } - // Default behavior here is just to send the message to the socket and - // return the number of bytes written - virtual int SendSimMessage(const std::string& message); +public: + std::string socket_connect_address_; + std::string socket_connect_port_; - protected: +protected: + virtual std::string GenerateResultsDataToSend(const GsResults &results); - tcp::socket* socket_ = nullptr; - boost::asio::io_context* io_context_ = nullptr; + virtual bool ProcessReceivedData(const std::string received_data); - std::unique_ptr receiver_thread_ = nullptr; + // Default behavior here is just to send the message to the socket and + // return the number of bytes written + virtual int SendSimMessage(const std::string &message); - // TBD - Is this thread safe? - bool receive_thread_exited_ = false; + // ---- state transition helper + hook ---- + void SetConnectionState(SimConnState s, const std::string &reason = "") { + SimConnState prev = + connection_state_.exchange(s, std::memory_order_relaxed); - boost::mutex sim_socket_receive_mutex_; - boost::mutex sim_socket_send_mutex_; - }; + { + boost::lock_guard lock(conn_state_mutex_); -} + // Clear stale error when we move into healthy-ish states + if (s == SimConnState::kConnected || s == SimConnState::kConnecting) { + last_connection_error_.clear(); + } + + // Store reason when provided (works for any state) + if (!reason.empty()) { + last_connection_error_ = reason; + } + } + + if (prev != s || !reason.empty()) { + OnConnectionStateChanged(prev, s, reason); + } + } + + // Derived classes can override to publish to ActiveMQ / UI / logs + virtual void OnConnectionStateChanged(SimConnState /*from*/, + SimConnState /*to*/, + const std::string & /*reason*/) {} + +protected: + tcp::socket *socket_ = nullptr; + boost::asio::io_context *io_context_ = nullptr; + + std::unique_ptr receiver_thread_ = nullptr; + + // TBD - Is this thread safe? + bool receive_thread_exited_ = false; + + boost::mutex sim_socket_receive_mutex_; + boost::mutex sim_socket_send_mutex_; + + // ---- state storage ---- + std::atomic connection_state_{SimConnState::kDisconnected}; + mutable boost::mutex conn_state_mutex_; + std::string last_connection_error_; +}; + +} // namespace golf_sim diff --git a/Software/LMSourceCode/ImageProcessing/libcamera_interface.cpp b/Software/LMSourceCode/ImageProcessing/libcamera_interface.cpp index 77935a8a..f720ef82 100644 --- a/Software/LMSourceCode/ImageProcessing/libcamera_interface.cpp +++ b/Software/LMSourceCode/ImageProcessing/libcamera_interface.cpp @@ -1,1629 +1,1845 @@ -/*****************************************************************//** - * \file libcamera_interface.cpp - * \brief Main interface to both PiTrac cameras (using the libcamera library) - * - * \author PiTrac - * \date February 2025 - *********************************************************************/ +/*****************************************************************/ /** + * \file + *libcamera_interface.cpp + * \brief + *Main + *interface + *to both + *PiTrac + *cameras + *(using the + *libcamera + *library) + * + * \author + *PiTrac + * \date + *February + *2025 + *********************************************************************/ /* SPDX-License-Identifier: GPL-2.0-only */ /* * Copyright (C) 2022-2025, Verdant Consultants, LLC. */ -#ifdef __unix__ // Ignore in Windows environment +#ifdef __unix__ // Ignore in Windows environment #include - #include // TBD - May need to be before ball_watcher, as there is a mman.h conflict #include "gs_ipc_system.h" - #include "ball_watcher.h" #include "ball_watcher_image_buffer.h" -#include "still_image_libcamera_app.hpp" #include "gs_club_data.h" +#include "still_image_libcamera_app.hpp" #include "image/image.hpp" #include #include -#include "gs_camera.h" #include "camera_hardware.h" -#include "gs_options.h" +#include "gs_camera.h" #include "gs_config.h" +#include "gs_options.h" #include "logging_tools.h" -#include -#include "motion_detect.h" -#include "libcamera_interface.h" #include "ball_image_proc.h" - +#include "libcamera_interface.h" +#include "motion_detect.h" +#include namespace golf_sim { - using lci = LibCameraInterface; - - - uint LibCameraInterface::kMaxWatchingCropWidth = 96; - uint LibCameraInterface::kMaxWatchingCropHeight = 88; - - double LibCameraInterface::kCamera1Gain = 6.0; - double LibCameraInterface::kCamera1Saturation = 1.0; - double LibCameraInterface::kCamera1HighFPSGain = 15.0; - double LibCameraInterface::kCamera1Contrast = 1.0; - double LibCameraInterface::kCamera2Gain = 6.0; - double LibCameraInterface::kCamera2Saturation = 1.0; - double LibCameraInterface::kCamera2ComparisonGain = 0.8; - double LibCameraInterface::kCamera2StrobedEnvironmentGain = 0.8; - double LibCameraInterface::kCamera2Contrast = 1.0; - double LibCameraInterface::kCamera2CalibrateOrLocationGain = 1.0; - double LibCameraInterface::kCamera2PuttingGain = 4.0; - double LibCameraInterface::kCamera2PuttingContrast = 1.0; - std::string LibCameraInterface::kCameraMotionDetectSettings = "./assets/motion_detect.json"; - - long LibCameraInterface::kCamera1StillShutterTimeuS = 15000; - long LibCameraInterface::kCamera2StillShutterTimeuS = 15000; - - // Default values are based on empirical measurements using a 6mm lens - int kCroppedImagePixelOffsetLeft = -5; - int kCroppedImagePixelOffsetUp = -13; - - // The system will start in a full-screen watching mode, but ensure - // we set it up once just in case - LibCameraInterface::CropConfiguration LibCameraInterface::camera_crop_configuration_ = kCropUnknown; - cv::Vec2i LibCameraInterface::current_watch_resolution_; - cv::Vec2i LibCameraInterface::current_watch_offset_; - - LibCameraInterface::CameraConfiguration LibCameraInterface::libcamera_configuration_[] = { LibCameraInterface::CameraConfiguration::kNotConfigured, LibCameraInterface::CameraConfiguration::kNotConfigured }; - - LibcameraJpegApp* LibCameraInterface::libcamera_app_[] = { nullptr, nullptr }; - - bool camera_location_found_ = false; - int previously_found_media_number_ = -1; - int previously_found_device_number_ = -1; - - void SetLibCameraLoggingOff() { - - // Unless we want REALLY detail information, just tell libcamera to be quiet, - // even for higher-level logging that libcamera might otherwise want to emit. +using lci = LibCameraInterface; + +uint LibCameraInterface::kMaxWatchingCropWidth = 96; +uint LibCameraInterface::kMaxWatchingCropHeight = 88; + +double LibCameraInterface::kCamera1Gain = 6.0; +double LibCameraInterface::kCamera1Saturation = 1.0; +double LibCameraInterface::kCamera1HighFPSGain = 15.0; +double LibCameraInterface::kCamera1Contrast = 1.0; +double LibCameraInterface::kCamera2Gain = 6.0; +double LibCameraInterface::kCamera2Saturation = 1.0; +double LibCameraInterface::kCamera2ComparisonGain = 0.8; +double LibCameraInterface::kCamera2StrobedEnvironmentGain = 0.8; +double LibCameraInterface::kCamera2Contrast = 1.0; +double LibCameraInterface::kCamera2Saturation = 0.0; +double LibCameraInterface::kCamera2CalibrateOrLocationGain = 1.0; +double LibCameraInterface::kCamera2PuttingGain = 4.0; +double LibCameraInterface::kCamera2PuttingContrast = 1.0; +std::string LibCameraInterface::kCameraMotionDetectSettings = + "./assets/motion_detect.json"; + +long LibCameraInterface::kCamera1StillShutterTimeuS = 15000; +long LibCameraInterface::kCamera2StillShutterTimeuS = 15000; + +// Default values are based on empirical measurements using a 6mm lens +int kCroppedImagePixelOffsetLeft = -5; +int kCroppedImagePixelOffsetUp = -13; + +// The system will start in a full-screen watching mode, but ensure +// we set it up once just in case +LibCameraInterface::CropConfiguration + LibCameraInterface::camera_crop_configuration_ = kCropUnknown; +cv::Vec2i LibCameraInterface::current_watch_resolution_; +cv::Vec2i LibCameraInterface::current_watch_offset_; + +LibCameraInterface::CameraConfiguration + LibCameraInterface::libcamera_configuration_[] = { + LibCameraInterface::CameraConfiguration::kNotConfigured, + LibCameraInterface::CameraConfiguration::kNotConfigured}; + +LibcameraJpegApp *LibCameraInterface::libcamera_app_[] = {nullptr, nullptr}; + +bool camera_location_found_ = false; +int previously_found_media_number_ = -1; +int previously_found_device_number_ = -1; + +void SetLibCameraLoggingOff() { + + // Unless we want REALLY detail information, just tell libcamera to be quiet, + // even for higher-level logging that libcamera might otherwise want to emit. + + if (GolfSimOptions::GetCommandLineOptions().logging_level_ != kTrace) { + libcamera::logSetTarget(libcamera::LoggingTargetNone); + + /* TBD - Not working, so avoid the extra log message for now + libcamera::logSetLevel("*", "ERROR"); + libcamera::logSetLevel("", "ERROR"); + */ + RPiCamApp::verbosity = 0; + } +} - if (GolfSimOptions::GetCommandLineOptions().logging_level_ != kTrace) { - libcamera::logSetTarget(libcamera::LoggingTargetNone); +/** + * \brief Once a ball has been identified in the image, this method will + * continuously watch the area where the ball is by taking images as quickly as + * possible and comparing each image to the prior image. See + * motion_detect_stage.cpp for details on detection. As soon as movement is + * detected, signals are sent to the camera 2 and strobe to take a picture. + * + * \param ball The teed-up ball that was previously located in the image + * \param image The image with the ball + * \param motion_detected Returns whether motion was detected at the time the + * method ended \return True iff no error occurred. + */ +bool WatchForHitAndTrigger(const GolfBall &ball, cv::Mat &image, + bool &motion_detected) { + + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot1CameraType; + const CameraHardware::LensType camera_lens_type = + GolfSimCamera::kSystemSlot1LensType; + const CameraHardware::CameraOrientation camera_orientation = + GolfSimCamera::kSystemSlot1CameraOrientation; + + // TBD - refactor this to get rid of the dummy camera necessity + GolfSimCamera c; + c.camera_hardware_.init_camera_parameters(GsCameraNumber::kGsCamera1, + camera_model, camera_lens_type, + camera_orientation); + + if (!WatchForBallMovement(c, ball, motion_detected)) { + GS_LOG_MSG(error, "Failed to WatchForBallMovement."); + return false; + } + + // We have access to the set of frames before and after the hit, so process + // club data here + + if (!GolfSimClubData::ProcessClubStrikeData(RecentFrames)) { + GS_LOG_MSG( + warning, + "Failed to GolfSimClubData::ProcessClubStrikeData(RecentFrames()."); + // TBD - Ignore for now + // return false; + } + + return true; +} - /* TBD - Not working, so avoid the extra log message for now - libcamera::logSetLevel("*", "ERROR"); - libcamera::logSetLevel("", "ERROR"); - */ - RPiCamApp::verbosity = 0; - } - } +bool LibCameraInterface::SendCamera2PreImage(const cv::Mat &raw_image) { + + // TBD -Not currently implemented + return false; + + /*** + // We must undistort here, because we are going to immediately send the + pre-image and the receiver + // may not know what camera (and what distortion matrix) is in use. + CameraHardware::CameraModel camera_model = CameraHardware::PiGS; + cv::Mat return_image = undistort_camera_image(raw_image, + GsCameraNumber::kGsCamera2, camera_model); + + // Send the image back to the cam1 system + GolfSimIPCMessage + ipc_message(GolfSimIPCMessage::IPCMessageType::kCamera2ReturnPreImage); + ipc_message.SetImageMat(return_image); + GolfSimIpcSystem::SendIpcMessage(ipc_message); + + // Save the image for later analysis + LoggingTools::LogImage("", return_image, std::vector < cv::Point >{}, true, + "log_cam2_last_pre_image.png"); + ***/ + return true; +} - /** - * \brief Once a ball has been identified in the image, this method will continuously watch the - * area where the ball is by taking images as quickly as possible and comparing each - * image to the prior image. See motion_detect_stage.cpp for details on detection. - * As soon as movement is detected, signals are sent to the camera 2 and strobe to take - * a picture. - * - * \param ball The teed-up ball that was previously located in the image - * \param image The image with the ball - * \param motion_detected Returns whether motion was detected at the time the method ended - * \return True iff no error occurred. - */ - bool WatchForHitAndTrigger(const GolfBall& ball, cv::Mat& image, bool& motion_detected) { - - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot1CameraType; - const CameraHardware::LensType camera_lens_type = GolfSimCamera::kSystemSlot1LensType; - const CameraHardware::CameraOrientation camera_orientation = GolfSimCamera::kSystemSlot1CameraOrientation; - - // TBD - refactor this to get rid of the dummy camera necessity - GolfSimCamera c; - c.camera_hardware_.init_camera_parameters(GsCameraNumber::kGsCamera1, camera_model, camera_lens_type, camera_orientation); - - if (!WatchForBallMovement(c, ball, motion_detected)) { - GS_LOG_MSG(error, "Failed to WatchForBallMovement."); - return false; - } +bool WatchForBallMovement(GolfSimCamera &camera, const GolfBall &ball, + bool &motion_detected) { - // We have access to the set of frames before and after the hit, so process - // club data here + if (!GolfSimClubData::Configure()) { + GS_LOG_TRACE_MSG(warning, "Failed to GolfSimClubData::Configure()"); + return false; + } - if (!GolfSimClubData::ProcessClubStrikeData(RecentFrames)) { - GS_LOG_MSG(warning, "Failed to GolfSimClubData::ProcessClubStrikeData(RecentFrames()."); - // TBD - Ignore for now - // return false; - } + // Setup the camera to watch at a high FPS by reducing the portion of the + // sensor that will be processed in each frame (cropping) - return true; - } + // Will be setup when camera is configured for cropping, then is used in the + // ball-watcher-loop + RPiCamEncoder app; - bool LibCameraInterface::SendCamera2PreImage(const cv::Mat& raw_image) { + if (!ConfigCameraForCropping(ball, camera, app)) { + GS_LOG_MSG(error, "Failed to ConfigCameraForCropping."); + return false; + } - // TBD -Not currently implemented - return false; + // Prepare the camera to watch the small ROI at a high frame rate + // This flag will be set here locally, but the sending of strobe + // pulses will be done within the motion-detection stage to reduce + // latency. + motion_detected = false; - /*** - // We must undistort here, because we are going to immediately send the pre-image and the receiver - // may not know what camera (and what distortion matrix) is in use. - CameraHardware::CameraModel camera_model = CameraHardware::PiGS; - cv::Mat return_image = undistort_camera_image(raw_image, GsCameraNumber::kGsCamera2, camera_model); - - // Send the image back to the cam1 system - GolfSimIPCMessage ipc_message(GolfSimIPCMessage::IPCMessageType::kCamera2ReturnPreImage); - ipc_message.SetImageMat(return_image); - GolfSimIpcSystem::SendIpcMessage(ipc_message); - - // Save the image for later analysis - LoggingTools::LogImage("", return_image, std::vector < cv::Point >{}, true, "log_cam2_last_pre_image.png"); - ***/ - return true; + try { + if (!ball_watcher_event_loop(app, motion_detected)) { + GS_LOG_MSG(error, "ball_watcher_event_loop failed to process."); } - - - bool WatchForBallMovement(GolfSimCamera& camera, const GolfBall& ball, bool& motion_detected) { - - if (!GolfSimClubData::Configure()) { - GS_LOG_TRACE_MSG(warning, "Failed to GolfSimClubData::Configure()"); - return false; - } - - // Setup the camera to watch at a high FPS by reducing the portion of the sensor that will - // be processed in each frame (cropping) - - // Will be setup when camera is configured for cropping, then is used in the ball-watcher-loop - RPiCamEncoder app; - - if (!ConfigCameraForCropping(ball, camera, app)) { - GS_LOG_MSG(error, "Failed to ConfigCameraForCropping."); - return false; - } - - // Prepare the camera to watch the small ROI at a high frame rate - // This flag will be set here locally, but the sending of strobe - // pulses will be done within the motion-detection stage to reduce - // latency. - motion_detected = false; - - try - { - if (!ball_watcher_event_loop(app, motion_detected)) { - GS_LOG_MSG(error, "ball_watcher_event_loop failed to process."); - } - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); - return false; - } - - uint frameIndex = 0; - unsigned int numFramesToShow = 10; - - if (motion_detected) { - std::string frame_information; - float average_frame_rate = 0.0; - float slowest_frame_rate = 10000.0; - float fastest_frame_rate = -10000.0; - - for (auto& it : boost::adaptors::reverse(RecentFrames)) { - cv::Mat& mostRecentFrameMat = it.mat; - - frame_information += "Frame " + std::to_string(frameIndex) + ": Framerate = " + std::to_string(it.frameRate) + "\n"; - average_frame_rate += it.frameRate; - - if (it.frameRate < slowest_frame_rate) { - slowest_frame_rate = it.frameRate; - } - - if (it.frameRate > fastest_frame_rate) { - fastest_frame_rate = it.frameRate; - } - - if (mostRecentFrameMat.empty()) { - GS_LOG_TRACE_MSG(warning, "Sequence No. " + std::to_string(it.requestSequence) + " was empty."); - } - - - frameIndex++; - } - - average_frame_rate /= RecentFrames.size(); - } - - return true; + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + return false; + } + + uint frameIndex = 0; + unsigned int numFramesToShow = 10; + + if (motion_detected) { + std::string frame_information; + float average_frame_rate = 0.0; + float slowest_frame_rate = 10000.0; + float fastest_frame_rate = -10000.0; + + for (auto &it : boost::adaptors::reverse(RecentFrames)) { + cv::Mat &mostRecentFrameMat = it.mat; + + frame_information += "Frame " + std::to_string(frameIndex) + + ": Framerate = " + std::to_string(it.frameRate) + + "\n"; + average_frame_rate += it.frameRate; + + if (it.frameRate < slowest_frame_rate) { + slowest_frame_rate = it.frameRate; + } + + if (it.frameRate > fastest_frame_rate) { + fastest_frame_rate = it.frameRate; + } + + if (mostRecentFrameMat.empty()) { + GS_LOG_TRACE_MSG(warning, "Sequence No. " + + std::to_string(it.requestSequence) + + " was empty."); + } + + frameIndex++; } + average_frame_rate /= RecentFrames.size(); + } + return true; +} - bool ConfigCameraForCropping(GolfBall ball, GolfSimCamera& camera, RPiCamEncoder& app) { - - // First, determine the cropping window size - - // watching_crop_width & Height defines the size of the cropping window, which will be a sub-region of the - // camera's full resolution. - float watching_crop_width = 0; - float watching_crop_height = 0; - - // If we're trying to get club strike image data, we'll need to expand the image size beyond - // whatever small cropping window we would otherwise have used. - // Doing so is likely to slow down the frame rate, however. - if (GolfSimClubData::kGatherClubData) { - watching_crop_width = GolfSimClubData::kClubImageWidthPixels; - watching_crop_height = GolfSimClubData::kClubImageHeightPixels; - } - else { - // If we're not trying to gather club-strike image data, use the largest - // cropping area that will still allow for the maximum FPS - watching_crop_width = LibCameraInterface::kMaxWatchingCropWidth; - watching_crop_height = LibCameraInterface::kMaxWatchingCropHeight; - } - - // One issue here is that the current GS camera will not crop any smaller than 98x88. So, if the ball is smaller - // than that, we still have to go with the 98x88 and deal with the smaller ball later within the ROI processing - - // We will later want to ensure the ball is not so small that the inscribed watching area ( for high FPS ) - // is larger than the ball and could pick up unrelated movement outside of the ball. - // We will deal with this when determining the ROI - uint largest_inscribed_square_side_length_of_ball = (double)(CvUtils::CircleRadius(ball.ball_circle_)) * sqrt(2); - - // Reduce the size of the inscribed square a little bit to ensure the motion detection ROI will be within the ball - largest_inscribed_square_side_length_of_ball *= 0.9; +bool ConfigCameraForCropping(GolfBall ball, GolfSimCamera &camera, + RPiCamEncoder &app) { + + // First, determine the cropping window size + + // watching_crop_width & Height defines the size of the cropping window, which + // will be a sub-region of the camera's full resolution. + float watching_crop_width = 0; + float watching_crop_height = 0; + + // If we're trying to get club strike image data, we'll need to expand the + // image size beyond whatever small cropping window we would otherwise have + // used. Doing so is likely to slow down the frame rate, however. + if (GolfSimClubData::kGatherClubData) { + watching_crop_width = GolfSimClubData::kClubImageWidthPixels; + watching_crop_height = GolfSimClubData::kClubImageHeightPixels; + } else { + // If we're not trying to gather club-strike image data, use the largest + // cropping area that will still allow for the maximum FPS + watching_crop_width = LibCameraInterface::kMaxWatchingCropWidth; + watching_crop_height = LibCameraInterface::kMaxWatchingCropHeight; + } + + // One issue here is that the current GS camera will not crop any smaller than + // 98x88. So, if the ball is smaller than that, we still have to go with the + // 98x88 and deal with the smaller ball later within the ROI processing + + // We will later want to ensure the ball is not so small that the inscribed + // watching area ( for high FPS ) is larger than the ball and could pick up + // unrelated movement outside of the ball. We will deal with this when + // determining the ROI + uint largest_inscribed_square_side_length_of_ball = + (double)(CvUtils::CircleRadius(ball.ball_circle_)) * sqrt(2); + + // Reduce the size of the inscribed square a little bit to ensure the motion + // detection ROI will be within the ball + largest_inscribed_square_side_length_of_ball *= 0.9; #ifdef NOT_DOING_THIS_NOW - uint largest_inscribed_square_side_length_of_ball = (double)(CvUtils::CircleRadius(ball.ball_circle_)) * sqrt(2); - GS_LOG_TRACE_MSG(trace, "largest_inscribed_square_side_length_of_ball is: " + std::to_string(largest_inscribed_square_side_length_of_ball)); - - // If we are not gathering club data, then the cropping window is a fixed size. And if that size - // is too large, reduce it to the size of the ball. - if (!GolfSimClubData::kGatherClubData) { - if (largest_inscribed_square_side_length_of_ball < watching_crop_width) { - GS_LOG_TRACE_MSG(trace, "Decreasing cropping window width because largest ball square side = " + std::to_string(largest_inscribed_square_side_length_of_ball)); - watching_crop_width = largest_inscribed_square_side_length_of_ball; - } - if (largest_inscribed_square_side_length_of_ball < watching_crop_height) { - GS_LOG_TRACE_MSG(trace, "Decreasing cropping window height because largest ball square side = " + std::to_string(largest_inscribed_square_side_length_of_ball)); - watching_crop_height = largest_inscribed_square_side_length_of_ball; - } - } + uint largest_inscribed_square_side_length_of_ball = + (double)(CvUtils::CircleRadius(ball.ball_circle_)) * sqrt(2); + GS_LOG_TRACE_MSG( + trace, "largest_inscribed_square_side_length_of_ball is: " + + std::to_string(largest_inscribed_square_side_length_of_ball)); + + // If we are not gathering club data, then the cropping window is a fixed + // size. And if that size is too large, reduce it to the size of the ball. + if (!GolfSimClubData::kGatherClubData) { + if (largest_inscribed_square_side_length_of_ball < watching_crop_width) { + GS_LOG_TRACE_MSG( + trace, + "Decreasing cropping window width because largest ball square side " + "= " + + std::to_string(largest_inscribed_square_side_length_of_ball)); + watching_crop_width = largest_inscribed_square_side_length_of_ball; + } + if (largest_inscribed_square_side_length_of_ball < watching_crop_height) { + GS_LOG_TRACE_MSG( + trace, + "Decreasing cropping window height because largest ball square side " + "= " + + std::to_string(largest_inscribed_square_side_length_of_ball)); + watching_crop_height = largest_inscribed_square_side_length_of_ball; + } + } #endif - // Starting with Pi 5, the crop height and width have to be divisible by 2. - // Enforce that here - watching_crop_width += ((int)watching_crop_width % 2); - watching_crop_height += ((int)watching_crop_height % 2); - - // TBD - After all that, and just for the current GS camera, we'll just set the cropping size to the smalllest possible size (for FPS) - // and either center the ball within that area, or put the ball in the bottom-right of the - // viewport if we want to see the club data. - - // Now determine the cropping window's offset within the full camera resolution image - // This offset will be based on the position of the ball - // The cropOffset is where, within the full-resolution image, the top-left corner of the - // cropping window is. - - float ball_x = CvUtils::CircleX(ball.ball_circle_); - float ball_y = CvUtils::CircleY(ball.ball_circle_); - - // Assume first is that the ball will be centered in the cropping window, then tweak - // it next if we're in club strike mode. Club strike imaging may require an offset. - // NOTE - the crop offset is from the bottom right! Not the top-left. - - float crop_offset_x = 0.0; - float crop_offset_y = 0.0; - - // TBD - Not sure why the cropped window has to be skewed like this, but otherwise, the ball - // is often off-center. - const float crop_offset_scale_adjustment_x = 1.0; // 0.99; - const float crop_offset_scale_adjustment_y = 1.0; //0.99; - - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kCroppedImagePixelOffsetLeft", kCroppedImagePixelOffsetLeft); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kCroppedImagePixelOffsetUp", kCroppedImagePixelOffsetUp); - - const float crop_offset_adjustment_x = kCroppedImagePixelOffsetLeft; // pixels - const float crop_offset_adjustment_y = kCroppedImagePixelOffsetUp; // pixels + // Starting with Pi 5, the crop height and width have to be divisible by 2. + // Enforce that here + watching_crop_width += ((int)watching_crop_width % 2); + watching_crop_height += ((int)watching_crop_height % 2); + + // TBD - After all that, and just for the current GS camera, we'll just set + // the cropping size to the smalllest possible size (for FPS) and either + // center the ball within that area, or put the ball in the bottom-right of + // the viewport if we want to see the club data. + + // Now determine the cropping window's offset within the full camera + // resolution image This offset will be based on the position of the ball The + // cropOffset is where, within the full-resolution image, the top-left corner + // of the cropping window is. + + float ball_x = CvUtils::CircleX(ball.ball_circle_); + float ball_y = CvUtils::CircleY(ball.ball_circle_); + + // Assume first is that the ball will be centered in the cropping window, then + // tweak it next if we're in club strike mode. Club strike imaging may require + // an offset. NOTE - the crop offset is from the bottom right! Not the + // top-left. + + float crop_offset_x = 0.0; + float crop_offset_y = 0.0; + + // TBD - Not sure why the cropped window has to be skewed like this, but + // otherwise, the ball is often off-center. + const float crop_offset_scale_adjustment_x = 1.0; // 0.99; + const float crop_offset_scale_adjustment_y = 1.0; // 0.99; + + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kCroppedImagePixelOffsetLeft", + kCroppedImagePixelOffsetLeft); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kCroppedImagePixelOffsetUp", + kCroppedImagePixelOffsetUp); + + const float crop_offset_adjustment_x = kCroppedImagePixelOffsetLeft; // pixels + const float crop_offset_adjustment_y = kCroppedImagePixelOffsetUp; // pixels + + // The video resolution is a little different than the still-photo resolution. + // So scale the center of the ball accordingly. + float x_scale = + crop_offset_scale_adjustment_x; // NOT IMPLEMENTED YET + // ((float)camera.camera_hardware_.video_resolution_x_ + // / + // (float)camera.camera_hardware_.resolution_x_); + crop_offset_x = crop_offset_adjustment_x + + (x_scale * (camera.camera_hardware_.resolution_x_ - + (ball_x + watching_crop_width / 2.0))); + + float y_scale = + crop_offset_scale_adjustment_y; // NOT IMPLEMENTED YET + // ((float)camera.camera_hardware_.video_resolution_y_ + // / + // (float)camera.camera_hardware_.resolution_y_); + crop_offset_y = crop_offset_adjustment_y + + (y_scale * (camera.camera_hardware_.resolution_y_ - + (ball_y + watching_crop_height / 2.0))); + + // If we're trying to get club images, then skew the cropping so that the ball + // ends up near the bottom-right such that the the golf ball "watch" ROI will + // eventually also be all the way at the bottom right (to give more room so + // see the club) + if (GolfSimClubData::kGatherClubData) { + crop_offset_x += (0.5 * watching_crop_width - + 0.5 * largest_inscribed_square_side_length_of_ball); + crop_offset_y += (0.5 * watching_crop_height - + 0.5 * largest_inscribed_square_side_length_of_ball); + } + + // Check for and correct if the resulting crop window would be outside the + // full resolution image If we need to correct something, preserve the crop + // width and correct the offset. NOTE - Camera resolutions are 1 greater than + // the greatest pixel position + if ((((camera.camera_hardware_.resolution_x_ - 1) - crop_offset_x) + + watching_crop_width) >= camera.camera_hardware_.resolution_x_) { + crop_offset_x = + (camera.camera_hardware_.video_resolution_x_ - crop_offset_x) - 1; + } + + if ((((camera.camera_hardware_.resolution_y_ - 1) - crop_offset_y) + + watching_crop_height) >= camera.camera_hardware_.resolution_y_) { + crop_offset_y = + (camera.camera_hardware_.video_resolution_y_ - crop_offset_y) - 1; + } + + cv::Vec2i watching_crop_size = + cv::Vec2i((uint)watching_crop_width, (uint)watching_crop_height); + cv::Vec2i watching_crop_offset = + cv::Vec2i((uint)crop_offset_x, (uint)crop_offset_y); + + // Check to see if this resolution is the same as we currently have + if (LibCameraInterface::camera_crop_configuration_ == + LibCameraInterface::kCropped && + LibCameraInterface::current_watch_resolution_ == watching_crop_size && + LibCameraInterface::current_watch_offset_ == watching_crop_offset) { + // Don't reset the crop if we don't need to. It takes time, especially the + // kernel-based cropping command lines + return true; + } + + // If the camera is flipped, the cropping has to be adjusted accordingly so + // that the crop offset is flipped vertically + GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + const CameraHardware::CameraOrientation camera_orientation = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraOrientation + : GolfSimCamera::kSystemSlot2CameraOrientation; + + if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { + GS_LOG_TRACE_MSG(trace, "Original watching_crop_offset[1] = " + + std::to_string(watching_crop_offset[1])); + float half_screen_height = + std::round(camera.camera_hardware_.video_resolution_y_ / 2); + watching_crop_offset[1] = + half_screen_height - (watching_crop_offset[1] - half_screen_height); + + // We essentially need to swap the top and bottom of the cropping rectangle + watching_crop_offset[1] -= watching_crop_size[1]; + + GS_LOG_TRACE_MSG(trace, "Flipped watching_crop_offset[1] = " + + std::to_string(watching_crop_offset[1])); + } + + if (!SendCameraCroppingCommand(camera, watching_crop_size, + watching_crop_offset)) { + GS_LOG_TRACE_MSG(error, "Failed to SendCameraCroppingCommand."); + return false; + } + + // TBD - Note - this entire thing should work on x,y vectors to the extent + // possible. + + // Determine what the resulting frame rate is in the resulting camera mode + // (and confirm the resolution) The camera would have been stopped after we + // took the first picture, so need re-start for this call + cv::Vec2i cropped_resolution; + uint cropped_frame_rate_fps; + if (!RetrieveCameraInfo(camera.camera_hardware_.camera_number_, + cropped_resolution, cropped_frame_rate_fps, true)) { + return false; + } + + if (!ConfigureLibCameraOptions(camera, app, watching_crop_size, + cropped_frame_rate_fps)) { + GS_LOG_TRACE_MSG(error, "Failed to ConfigureLibCameraOptions."); + return false; + } + + // For the post processing, we also need to know what portion of the cropped + // window is of interest in terms of determining ball movement. Unlike the + // cropping window, Offsets here are from the top-left corner of the cropped + // window Assume the ball is perfectly round, so the roi is square. We don't + // want to watch for movement anywhere but within the ball. NOTE - We have to + // convert from the center of the ROI to the top-left + + float roi_offset_x = 0.0; + float roi_offset_y = 0.0; + + float roi_size_x = 0.0; + float roi_size_y = 0.0; + + float size_difference_x = + largest_inscribed_square_side_length_of_ball - watching_crop_width; + float size_difference_y = + largest_inscribed_square_side_length_of_ball - watching_crop_height; + + float flipped_orientation_multiplier = 1.0; + + if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { + flipped_orientation_multiplier = -1.0; + } + if (size_difference_x >= 0.0) { + // The cropped area is already fully inside the ball (assuming we're not + // dealing with club strike data so just have the ROI match the cropping + // area + roi_size_x = watching_crop_width; + roi_offset_x = 0.0; + } else { + // The cropping area is larger than a square inscribed in the ball circle, + // so we want to focus the ROI on just the area of that square. + roi_size_x = largest_inscribed_square_side_length_of_ball * x_scale; + // Essentially center the ROI within the image, assuming that the ball is + // centered in the cropping area (which will likely not be the case if we + // are widening the cropping area for club strike data) + roi_offset_x = -0.5 * (roi_size_x - watching_crop_width); + } + + if (size_difference_y >= 0.0) { + // The cropped area is already fully inside the ball (assuming we're not + // dealing with club strike data so just have the ROI match the cropping + // area + roi_size_y = watching_crop_height; + roi_offset_y = 0.0; + } else { + // The cropping area is larger than a square inscribed in the ball circle, + // so we want to focus the ROI on just the area of that square. + roi_size_y = largest_inscribed_square_side_length_of_ball * y_scale; + // Essentially center the ROI within the image, assuming that the ball is + // centered in the cropping area (which will likely not be the case if we + // are widening the cropping area for club strike data) + roi_offset_y = -0.5 * (roi_size_y - watching_crop_height); + roi_offset_y *= flipped_orientation_multiplier; + } + + // If the camera is reversed, we need to move the offset in the opposite + // direction of the original offset + if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { + GS_LOG_TRACE_MSG(trace, + "Original roi_offset_y = " + std::to_string(roi_offset_y)); + roi_offset_y = -roi_offset_y; + GS_LOG_TRACE_MSG(trace, + "Updated roi_offset_y = " + std::to_string(roi_offset_y)); + } + + roi_offset_x = std::max(roi_offset_x, 0.0f); + roi_offset_y = std::max(roi_offset_y, 0.0f); + + if (camera.camera_hardware_.video_resolution_x_ < 0 || + camera.camera_hardware_.video_resolution_y_ < 0) { + GS_LOG_TRACE_MSG(error, "camera.camera_hardware_.video_resolution_x_ or " + "_y_ have not been set. Exiting."); + return false; + } + + roi_offset_x = std::min(roi_offset_x, + (float)camera.camera_hardware_.video_resolution_x_); + roi_offset_y = std::min(roi_offset_y, + (float)camera.camera_hardware_.video_resolution_y_); + + cv::Vec2i roi_offset = cv::Vec2i((int)roi_offset_x, (int)roi_offset_y); + cv::Vec2i roi_size = cv::Vec2i((uint)roi_size_x, (uint)roi_size_y); + + if (!ConfigurePostProcessing(roi_size, roi_offset)) { + GS_LOG_TRACE_MSG(error, "Failed to ConfigurePostProcessing."); + return false; + } + + // Save the current cropping setup in hopes that we might be able to + // avoid another media-ctl call next time if we are going to use the same + // values next time. + LibCameraInterface::current_watch_resolution_ = watching_crop_size; + LibCameraInterface::current_watch_offset_ = watching_crop_offset; + + // Signal that the cropping setup has changed so that we know to change it + // back to full-screen later when we're watching for the ball to first + // appear.. + LibCameraInterface::camera_crop_configuration_ = LibCameraInterface::kCropped; + + return true; +} - // The video resolution is a little different than the still-photo resolution. - // So scale the center of the ball accordingly. - float x_scale = crop_offset_scale_adjustment_x; // NOT IMPLEMENTED YET ((float)camera.camera_hardware_.video_resolution_x_ / (float)camera.camera_hardware_.resolution_x_); - crop_offset_x = crop_offset_adjustment_x + (x_scale * (camera.camera_hardware_.resolution_x_ - (ball_x + watching_crop_width / 2.0))); +bool DiscoverCameraLocation(const GsCameraNumber camera_number, + int &media_number, int &device_number) { - float y_scale = crop_offset_scale_adjustment_y; // NOT IMPLEMENTED YET ((float)camera.camera_hardware_.video_resolution_y_ / (float)camera.camera_hardware_.resolution_y_); - crop_offset_y = crop_offset_adjustment_y + (y_scale * (camera.camera_hardware_.resolution_y_ - (ball_y + watching_crop_height / 2.0))); + // The camera location won't change during the course of a single exceution, + // so no need to figure this out more than once - re-use the earlier values if + // we can + if (camera_location_found_) { + media_number = previously_found_media_number_; + device_number = previously_found_device_number_; - // If we're trying to get club images, then skew the cropping so that the ball ends up near the - // bottom-right such that the the golf ball "watch" ROI will eventually also be - // all the way at the bottom right (to give more room so see the club) - if (GolfSimClubData::kGatherClubData) { - crop_offset_x += (0.5 * watching_crop_width - 0.5 * largest_inscribed_square_side_length_of_ball); - crop_offset_y += (0.5 * watching_crop_height - 0.5 * largest_inscribed_square_side_length_of_ball); - } + return true; + } + + // Otherwise, go out and search all of the possible places to search for the + // camera + const std::string pitrac_root = std::getenv("PITRAC_ROOT"); - // Check for and correct if the resulting crop window would be outside the full resolution image - // If we need to correct something, preserve the crop width and correct the offset. - // NOTE - Camera resolutions are 1 greater than the greatest pixel position - if ((((camera.camera_hardware_.resolution_x_ - 1) - crop_offset_x) + watching_crop_width) >= camera.camera_hardware_.resolution_x_) { - crop_offset_x = (camera.camera_hardware_.video_resolution_x_ - crop_offset_x) - 1; - } + if (pitrac_root.empty()) { + GS_LOG_TRACE_MSG(error, "DiscoverCameraLocation - could not get " + "PITRAC_ROOT environment variable"); + return false; + } + + const std::string kOutputFileName = "/tmp/pi_cam_location.txt"; - if ((((camera.camera_hardware_.resolution_y_ - 1) - crop_offset_y) + watching_crop_height) >= camera.camera_hardware_.resolution_y_) { - crop_offset_y = (camera.camera_hardware_.video_resolution_y_ - crop_offset_y) - 1; - } + std::string s; - cv::Vec2i watching_crop_size = cv::Vec2i((uint)watching_crop_width, (uint)watching_crop_height); - cv::Vec2i watching_crop_offset = cv::Vec2i((uint)crop_offset_x, (uint)crop_offset_y); + s += "#!/bin/bash\n"; + s += "rm -f /tmp/discover_media.txt /tmp/discover_device.txt " + "/tmp/discover_result.txt " + + kOutputFileName + "\n"; + s += "for ((m = 0; m <= 5; ++m))\n"; + s += " do\n"; + s += " rm -f /tmp/discover_result.txt\n"; + s += " media-ctl -d \"/dev/media$m\" --print-dot | grep imx > " + "/tmp/discover_media.txt\n"; + s += " awk -F\"imx296 \" '{print $2}' < /tmp/discover_media.txt | cut " + "-d- -f1 > /tmp/discover_device.txt\n"; + s += " echo -n -e \"$m \" > /tmp/discover_result.txt\n"; + s += " cat /tmp/discover_device.txt >> /tmp/discover_result.txt\n"; - // Check to see if this resolution is the same as we currently have - if (LibCameraInterface::camera_crop_configuration_ == LibCameraInterface::kCropped && - LibCameraInterface::current_watch_resolution_ == watching_crop_size && - LibCameraInterface::current_watch_offset_ == watching_crop_offset - ) { - // Don't reset the crop if we don't need to. It takes time, especially the kernel-based cropping command lines - return true; - } + s += " if grep imx /tmp/discover_media.txt > /dev/null; then cat " + "/tmp/discover_result.txt >> " + + kOutputFileName + "; fi\n"; + s += " done\n"; - // If the camera is flipped, the cropping has to be adjusted accordingly so that the crop offset is flipped vertically - GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; - const CameraHardware::CameraOrientation camera_orientation = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraOrientation : GolfSimCamera::kSystemSlot2CameraOrientation; + s += " rm -f /tmp/discover_media.txt /tmp/discover_device.txt " + "/tmp/discover_result.txt\n"; - if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { - GS_LOG_TRACE_MSG(trace, "Original watching_crop_offset[1] = " + std:: to_string(watching_crop_offset[1])); - float half_screen_height = std::round(camera.camera_hardware_.video_resolution_y_ / 2); - watching_crop_offset[1] = half_screen_height - (watching_crop_offset[1] - half_screen_height); + const std::string script_name = "/tmp/pi_cam_location.sh"; - // We essentially need to swap the top and bottom of the cropping rectangle - watching_crop_offset[1] -= watching_crop_size[1]; + // Ensure that we can write to the output file if it was already created + std::string script_command = "sudo rm " + script_name; + system(script_command.c_str()); - GS_LOG_TRACE_MSG(trace, "Flipped watching_crop_offset[1] = " + std::to_string(watching_crop_offset[1])); - } + int cmdResult = system(script_command.c_str()); - if (!SendCameraCroppingCommand(camera, watching_crop_size, watching_crop_offset)) { - GS_LOG_TRACE_MSG(error, "Failed to SendCameraCroppingCommand."); - return false; - } + // It's ok if the file wasn't there. No need to check the return code + // Write the script out to file to run. + // Otherwise, system() would try to run the script as a sh script, + // not a bash script + std::ofstream script_file(script_name); // Open file for reading - // TBD - Note - this entire thing should work on x,y vectors to the extent possible. + if (!script_file.is_open()) { + GS_LOG_TRACE_MSG(error, + "DiscoverCameraLocation - failed to open script file " + + script_name); + return false; + } - // Determine what the resulting frame rate is in the resulting camera mode (and confirm the resolution) - // The camera would have been stopped after we took the first picture, so need re-start for this call - cv::Vec2i cropped_resolution; - uint cropped_frame_rate_fps; - if (!RetrieveCameraInfo(camera.camera_hardware_.camera_number_, cropped_resolution, cropped_frame_rate_fps, true)) { - return false; - } + // Write the script to the file + script_file << s << std::endl; + script_file.close(); - if (!ConfigureLibCameraOptions(camera, app, watching_crop_size, cropped_frame_rate_fps)) { - GS_LOG_TRACE_MSG(error, "Failed to ConfigureLibCameraOptions."); - return false; - } + // At least currently, we need to make the script file executable before + // calling it + script_command = "sudo chmod 777 " + script_name; + system(script_command.c_str()); - // For the post processing, we also need to know what portion of the cropped window - // is of interest in terms of determining ball movement. - // Unlike the cropping window, Offsets here are from the top-left corner of the cropped window - // Assume the ball is perfectly round, so the roi is square. We don't want to watch for movement - // anywhere but within the ball. - // NOTE - We have to convert from the center of the ROI to the top-left + script_command = script_name; - float roi_offset_x = 0.0; - float roi_offset_y = 0.0; + cmdResult = system(script_command.c_str()); - float roi_size_x = 0.0; - float roi_size_y = 0.0; + if (cmdResult != 0) { + GS_LOG_TRACE_MSG( + error, "system(DiscoverCameraLocation) failed. Return value was: " + + std::to_string(cmdResult)); + return false; + } - float size_difference_x = largest_inscribed_square_side_length_of_ball - watching_crop_width; - float size_difference_y = largest_inscribed_square_side_length_of_ball - watching_crop_height; + // Read and parse the output results + std::ifstream file(kOutputFileName); - float flipped_orientation_multiplier = 1.0; + if (!file.is_open()) { + GS_LOG_TRACE_MSG(error, + "DiscoverCameraLocation - failed to open output file " + + kOutputFileName); + return false; + } - if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { - flipped_orientation_multiplier = -1.0; - } - if (size_difference_x >= 0.0) { - // The cropped area is already fully inside the ball (assuming we're not dealing with club strike data - // so just have the ROI match the cropping area - roi_size_x = watching_crop_width; - roi_offset_x = 0.0; - } - else { - // The cropping area is larger than a square inscribed in the ball circle, so we want to focus the - // ROI on just the area of that square. - roi_size_x = largest_inscribed_square_side_length_of_ball * x_scale; - // Essentially center the ROI within the image, assuming that the ball is centered in the cropping area - // (which will likely not be the case if we are widening the cropping area for club strike data) - roi_offset_x = -0.5 * (roi_size_x - watching_crop_width); - } + std::string line; - if (size_difference_y >= 0.0) { - // The cropped area is already fully inside the ball (assuming we're not dealing with club strike data - // so just have the ROI match the cropping area - roi_size_y = watching_crop_height; - roi_offset_y = 0.0; - } - else { - // The cropping area is larger than a square inscribed in the ball circle, so we want to focus the - // ROI on just the area of that square. - roi_size_y = largest_inscribed_square_side_length_of_ball * y_scale; - // Essentially center the ROI within the image, assuming that the ball is centered in the cropping area - // (which will likely not be the case if we are widening the cropping area for club strike data) - roi_offset_y = -0.5 * (roi_size_y - watching_crop_height); - roi_offset_y *= flipped_orientation_multiplier; - } + std::stringstream buffer; + buffer << file.rdbuf(); - // If the camera is reversed, we need to move the offset in the opposite direction of the original offset - if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { - GS_LOG_TRACE_MSG(trace, "Original roi_offset_y = " + std::to_string(roi_offset_y)); - roi_offset_y = -roi_offset_y; - GS_LOG_TRACE_MSG(trace, "Updated roi_offset_y = " + std::to_string(roi_offset_y)); - } + line = buffer.str(); - roi_offset_x = std::max(roi_offset_x, 0.0f); - roi_offset_y = std::max(roi_offset_y, 0.0f); + // Read only one line + if (line.empty()) { + GS_LOG_TRACE_MSG(error, "system(DiscoverCameraLocation) failed."); + return false; + } - if (camera.camera_hardware_.video_resolution_x_ < 0 || camera.camera_hardware_.video_resolution_y_ < 0) { - GS_LOG_TRACE_MSG(error, "camera.camera_hardware_.video_resolution_x_ or _y_ have not been set. Exiting."); - return false; - } + file.close(); - roi_offset_x = std::min(roi_offset_x, (float)camera.camera_hardware_.video_resolution_x_); - roi_offset_y = std::min(roi_offset_y, (float)camera.camera_hardware_.video_resolution_y_); + // The format of the output file should be + // + try { + // Using a single pi requires both cameras to be connected to that Pi. + // If we are not receiving two sets of camera data, then something is wrong. + int new_line_position = line.find('\n'); - cv::Vec2i roi_offset = cv::Vec2i((int)roi_offset_x, (int)roi_offset_y); - cv::Vec2i roi_size = cv::Vec2i((uint)roi_size_x, (uint)roi_size_y); + if (new_line_position == (int)string::npos) { - if (!ConfigurePostProcessing(roi_size, roi_offset)) { - GS_LOG_TRACE_MSG(error, "Failed to ConfigurePostProcessing."); - return false; - } + // There is only one line of discovered information + + if (GolfSimOptions::GetCommandLineOptions().run_single_pi_) { + GS_LOG_TRACE_MSG( + error, "No expected new line found in camera location output. " + "Missing camera when running in single-pi mode."); + return false; + } else { + // We have only a single line of information. Do not need to do + // anything else + } + } else { + // Assume (TBD - Confirm with Pi people) that the camera on camera unit 0 + // (the port nearest the LAN port) will correspond to the first line of + // the returned media-ctl output + + if (camera_number == GsCameraNumber::kGsCamera1) { + // Gee the information from the first line + std::string first_line_str = line.substr(0, new_line_position); + line = first_line_str; + } else { + // Gee the information from the second line + std::string first_line_str = line.substr(new_line_position + 1); + line = first_line_str; + } + } + // Parse out the media and device numbers from what should be the first line + // of the media-ctl location report - // Save the current cropping setup in hopes that we might be able to - // avoid another media-ctl call next time if we are going to use the same - // values next time. - LibCameraInterface::current_watch_resolution_ = watching_crop_size; - LibCameraInterface::current_watch_offset_ = watching_crop_offset; + int last_space_position = line.rfind(' '); - // Signal that the cropping setup has changed so that we know to change it - // back to full-screen later when we're watching for the ball to first appear.. - LibCameraInterface::camera_crop_configuration_ = LibCameraInterface::kCropped; + std::string device_number_str; - return true; + if (last_space_position != (int)string::npos) { + device_number_str = line.substr(last_space_position + 1); + } else { + GS_LOG_TRACE_MSG(error, "No space found"); + return false; } + int first_space_position = line.find(' '); -bool DiscoverCameraLocation(const GsCameraNumber camera_number, int& media_number, int& device_number) { - - // The camera location won't change during the course of a single exceution, so - // no need to figure this out more than once - re-use the earlier values if we can - if (camera_location_found_) { - media_number = previously_found_media_number_; - device_number = previously_found_device_number_; + std::string media_number_str; - return true; + if (first_space_position != (int)string::npos) { + media_number_str = line.substr(0, first_space_position); + } else { + GS_LOG_TRACE_MSG(error, "No space found"); + return false; } - // Otherwise, go out and search all of the possible places to search for the camera - const std::string pitrac_root = std::getenv("PITRAC_ROOT"); - - if (pitrac_root.empty()) { - GS_LOG_TRACE_MSG(error, "DiscoverCameraLocation - could not get PITRAC_ROOT environment variable"); - return false; + if (media_number_str.empty() || device_number_str.empty()) { + GS_LOG_TRACE_MSG(error, + "Failed to parse media and device number strings"); + return false; } - const std::string kOutputFileName = "/tmp/pi_cam_location.txt"; - - std::string s; - - s += "#!/bin/bash\n"; - s += "rm -f /tmp/discover_media.txt /tmp/discover_device.txt /tmp/discover_result.txt " + kOutputFileName + "\n"; - s += "for ((m = 0; m <= 5; ++m))\n"; - s += " do\n"; - s += " rm -f /tmp/discover_result.txt\n"; - s += " media-ctl -d \"/dev/media$m\" --print-dot | grep imx > /tmp/discover_media.txt\n"; - s += " awk -F\"imx296 \" '{print $2}' < /tmp/discover_media.txt | cut -d- -f1 > /tmp/discover_device.txt\n"; - s += " echo -n -e \"$m \" > /tmp/discover_result.txt\n"; - s += " cat /tmp/discover_device.txt >> /tmp/discover_result.txt\n"; - - s += " if grep imx /tmp/discover_media.txt > /dev/null; then cat /tmp/discover_result.txt >> " + kOutputFileName + "; fi\n"; - s += " done\n"; + media_number = std::stoi(media_number_str); + device_number = std::stoi(device_number_str); + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + return false; + } - s += " rm -f /tmp/discover_media.txt /tmp/discover_device.txt /tmp/discover_result.txt\n"; + // Signal that we won't need to do this again during this run. + camera_location_found_ = true; + previously_found_media_number_ = media_number; + previously_found_device_number_ = device_number; - const std::string script_name = "/tmp/pi_cam_location.sh"; + return true; +} - // Ensure that we can write to the output file if it was already created - std::string script_command = "sudo rm " + script_name; - system(script_command.c_str()); +bool SendCameraCroppingCommand(const GolfSimCamera &camera, + cv::Vec2i &cropping_window_size, + cv::Vec2i &cropping_window_offset) { - int cmdResult = system(script_command.c_str()); + std::string mediaCtlCmd = GetCmdLineForMediaCtlCropping( + camera, cropping_window_size, cropping_window_offset); + int cmdResult = system(mediaCtlCmd.c_str()); - // It's ok if the file wasn't there. No need to check the return code - - // Write the script out to file to run. - // Otherwise, system() would try to run the script as a sh script, - // not a bash script - std::ofstream script_file(script_name); // Open file for reading + if (cmdResult != 0) { + GS_LOG_TRACE_MSG(error, "system(mediaCtlCmd) failed."); + return false; + } + return true; +} - if (!script_file.is_open()) { - GS_LOG_TRACE_MSG(error, "DiscoverCameraLocation - failed to open script file " + script_name); - return false; - } +bool ConfigurePostProcessing(const cv::Vec2i &roi_size, + const cv::Vec2i &roi_offset) { + + float kDifferenceM = 0.; + float kDifferenceC = 0.; + float kRegionThreshold = 0.; + float kMaxRegionThreshold = 0.; + uint kFramePeriod = 0; + uint kHSkip = 0; + uint kVSkip = 0; + + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kDifferenceM", kDifferenceM); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kDifferenceC", kDifferenceC); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kRegionThreshold", kRegionThreshold); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kMaxRegionThreshold", kMaxRegionThreshold); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kFramePeriod", kFramePeriod); + GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kHSkip", + kHSkip); + GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kVSkip", + kVSkip); + + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kCroppedImagePixelOffsetLeft", + kCroppedImagePixelOffsetLeft); + GolfSimConfiguration::SetConstant( + "gs_config.motion_detect_stage.kCroppedImagePixelOffsetUp", + kCroppedImagePixelOffsetUp); + + // These values will be used within the motion-detect post-processing + + MotionDetectStage::incoming_configuration.use_incoming_configuration = + true; // Don't use .json file values -- use the following + + MotionDetectStage::incoming_configuration.roi_x = roi_offset[0]; + MotionDetectStage::incoming_configuration.roi_y = roi_offset[1]; + + MotionDetectStage::incoming_configuration.roi_width = roi_size[0]; + MotionDetectStage::incoming_configuration.roi_height = roi_size[1]; + + MotionDetectStage::incoming_configuration.difference_m = kDifferenceM; + MotionDetectStage::incoming_configuration.difference_c = kDifferenceC; + MotionDetectStage::incoming_configuration.region_threshold = kRegionThreshold; + MotionDetectStage::incoming_configuration.max_region_threshold = + kMaxRegionThreshold; + MotionDetectStage::incoming_configuration.frame_period = kFramePeriod; + MotionDetectStage::incoming_configuration.hskip = + kHSkip; // TBD - don't hard code the skip factor + MotionDetectStage::incoming_configuration.vskip = kVSkip; + MotionDetectStage::incoming_configuration.verbose = 2; + MotionDetectStage::incoming_configuration.showroi = true; + + return true; +} - // Write the script to the file - script_file << s << std::endl; - script_file.close(); +bool ConfigureLibCameraOptions(const GolfSimCamera &camera, RPiCamEncoder &app, + const cv::Vec2i &cropping_window_size, + uint cropped_frame_rate_fps) { + + GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + + VideoOptions *options = app.GetOptions(); + + char dummy_arguments[] = "DummyExecutableName"; + char *argv[] = {dummy_arguments, NULL}; + + if (!options->Parse(1, argv)) { + GS_LOG_TRACE_MSG(trace, "failed to parse dummy command line."); + return false; + } + + SetLibCameraLoggingOff(); + + options->no_raw = + true; // See https://forums.raspberrypi.com/viewtopic.php?t=369927 - + // cameras won't work unless this is set. + + std::string shutter_speed_string; + // Generally need to crank up gain due to short exposure time at high FPS. + float camera_gain = 0.0; + + if (GolfSimClubData::kGatherClubData) { + camera_gain = GolfSimClubData::kClubImageCameraGain; + shutter_speed_string = + std::to_string((int)(GolfSimClubData::kClubImageShutterSpeedMultiplier * + (1. / cropped_frame_rate_fps * 1000000.))) + + "us"; // TBD - should be 1,000,000 for uS setting + } else { + camera_gain = LibCameraInterface::kCamera1HighFPSGain; + shutter_speed_string = + std::to_string((int)(1. / cropped_frame_rate_fps * 1000000.)) + + "us"; // TBD - should be 1,000,000 for uS setting + } + + options->gain = camera_gain; + options->shutter.set( + shutter_speed_string); // TBD - should be 1,000,000 for uS setting + + options->saturation = (camera_number == GsCameraNumber::kGsCamera1) + ? LibCameraInterface::kCamera1Saturation + : LibCameraInterface::kCamera2Saturation; + + GS_LOG_MSG(trace, "Saturation = " + std::to_string(options->saturation)); + + options->timeout.set("0ms"); + + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot1CameraType; + if (camera_model != CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { + options->denoise = "cdn_off"; + } else { + options->denoise = "auto"; + } + + GS_LOG_TRACE_MSG(trace, "Camera denoise option set to: " + options->denoise); + options->framerate = cropped_frame_rate_fps; + options->nopreview = true; + options->lores_width = 0; + options->lores_height = 0; + options->viewfinder_width = 0; + options->viewfinder_height = 0; + options->info_text = ""; + options->level = "4.2"; + + const CameraHardware::CameraOrientation camera_orientation = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraOrientation + : GolfSimCamera::kSystemSlot2CameraOrientation; + + if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { + // Tell libcamera to flip the image vertically back to where it should be + options->transform = libcamera::Transform::VFlip; + GS_LOG_MSG(trace, "Flipping still picture upside down."); + } else { + GS_LOG_MSG(trace, "NOT flipping still picture upside down."); + } + + // On the Pi5, there's no hardware H.264 encoding, so let's try to turn it off + // entirely TBD - See video_options.cpp to consider other options like libav + options->codec = "yuv420"; // was h.264, but that no longer works on Pi5 + + if (GolfSimConfiguration::GetPiModel() == + GolfSimConfiguration::PiModel::kRPi5) { + options->tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296.json"; + } else { + options->tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296.json"; + } + setenv("LIBCAMERA_RPI_TUNING_FILE", options->tuning_file.c_str(), 1); + + // TBD - We are switching away from having the post_process_file trigger the + // dynamic loading of the motion_detection module. Instead, hop[efully for + // speed, we will use a statically-bound motion_detection module. See + // ball_watcher.cpp options->post_process_file = + // LibCameraInterface::kCameraMotionDetectSettings; + + if (cropping_window_size[0] > 0 && cropping_window_size[1] > 0) { + options->width = cropping_window_size[0]; + options->height = cropping_window_size[1]; + } + + if (options->verbose >= 2) + options->Print(); + + return true; +} - // At least currently, we need to make the script file executable before calling it - script_command = "sudo chmod 777 " + script_name; - system(script_command.c_str()); +// For example, to set the GS cam back to its default, use "(0, 0)/1456x1088" +// 98x88 can deliver 572 FPS on the GS cam. +std::string GetCmdLineForMediaCtlCropping(const GolfSimCamera &camera, + cv::Vec2i croppedHW, + cv::Vec2i crop_offset_xY) { + + std::string s; + + int media_number = -1; + int device_number = -1; + + if (!DiscoverCameraLocation(camera.camera_hardware_.camera_number_, + media_number, device_number)) { + GS_LOG_MSG(error, "Could not DiscoverCameraLocation"); + return ""; + } + + // The format will be different for mono cameras amd cp;pr + std::string format = + (camera.camera_hardware_.camera_is_mono()) ? "Y10_1X10" : "SBGGR10_1X10"; + + s += "#!/bin/sh\n"; + s += "if media-ctl -d \"/dev/media" + std::to_string(media_number) + + "\" --set-v4l2 \"'imx296 " + std::to_string(device_number) + + "-001a':0 [fmt:" + format + "/" + std::to_string(croppedHW[0]) + "x" + + std::to_string(croppedHW[1]) + " crop:(" + + std::to_string(crop_offset_xY[0]) + "," + + std::to_string(crop_offset_xY[1]) + ")/" + std::to_string(croppedHW[0]) + + "x" + std::to_string(croppedHW[1]) + + "]\" > /dev/null; then echo -e \"/dev/media" + + std::to_string(media_number) + "\" > /dev/null; break; fi\n"; + + return s; +} - script_command = script_name; +bool RetrieveCameraInfo(const GsCameraNumber camera_number, + cv::Vec2i &resolution, uint &frameRate, + bool restartCamera) { - cmdResult = system(script_command.c_str()); + LibcameraJpegApp app; - if (cmdResult != 0) { - GS_LOG_TRACE_MSG(error, "system(DiscoverCameraLocation) failed. Return value was: " + std::to_string(cmdResult)); - return false; - } - - // Read and parse the output results - std::ifstream file(kOutputFileName); + if (restartCamera) { - if (!file.is_open()) { - GS_LOG_TRACE_MSG(error, "DiscoverCameraLocation - failed to open output file " + kOutputFileName); - return false; - } + try { + StillOptions *options = app.GetOptions(); - std::string line; + char dummy_arguments[] = "DummyExecutableName"; + char *argv[] = {dummy_arguments, NULL}; - std::stringstream buffer; - buffer << file.rdbuf(); + if (options->Parse(1, argv)) { + if (options->verbose >= 2) + options->Print(); - line = buffer.str(); + SetLibCameraLoggingOff(); - // Read only one line - if (line.empty()) { - GS_LOG_TRACE_MSG(error, "system(DiscoverCameraLocation) failed."); - return false; + options->no_raw = + true; // See https://forums.raspberrypi.com/viewtopic.php?t=369927 + + // Set the camera number (0 or 1, likely) when we have more than one + // camera + options->camera = + (camera_number == GsCameraNumber::kGsCamera1 || + !GolfSimOptions::GetCommandLineOptions().run_single_pi_) + ? 0 + : 1; + + // Get the camera open for a moment so that we can read its settings + app.OpenCamera(); + // GS_LOG_TRACE_MSG(trace, "About to ConfigureViewfinder"); + app.ConfigureViewfinder(); + // GS_LOG_TRACE_MSG(trace, "About to StartCamera"); + app.StartCamera(); + // GS_LOG_TRACE_MSG(trace, "About to StopCamera"); + app.StopCamera(); + } + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + return false; } - - file.close(); + } + + std::vector> cameras = app.GetCameras(); + + if (cameras.size() == 0) { + GS_LOG_MSG(error, "Could not getCameras."); + return false; + } + + int camera_slot_number = + (camera_number == GsCameraNumber::kGsCamera1 || + !GolfSimOptions::GetCommandLineOptions().run_single_pi_) + ? 0 + : 1; + + auto const &cam = cameras[camera_slot_number]; + + std::unique_ptr config = + cam->generateConfiguration({libcamera::StreamRole::Raw}); + if (!config) { + GS_LOG_MSG(error, "Could not get cam config."); + return false; + } + + // Parse out the frames-per-second value (FPS) + auto fd_ctrl = cam->controls().find(&controls::FrameDurationLimits); + auto crop_ctrl = cam->properties().get(properties::ScalerCropMaximum); + libcamera::Rectangle cropRect = crop_ctrl.value(); + double fps = fd_ctrl == cam->controls().end() + ? NAN + : (1e6 / fd_ctrl->second.min().get()); + std::cout << std::fixed << std::setprecision(2) << "[" << fps << " fps - " + << crop_ctrl->toString() << " crop" << "]\n"; + + // Return results + resolution = cv::Vec2i(cropRect.width, cropRect.height); + frameRate = fps; + + return true; +} - // The format of the output file should be - // - try { +cv::Mat +LibCameraInterface::undistort_camera_image(const cv::Mat &img, + const GolfSimCamera &camera) { + + if (!camera.camera_hardware_.use_undistortion_matrix_) { + GS_LOG_MSG(trace, "undistort_camera_image ignoring camera with no " + "undistortion matrix. Returning original image."); + return img; + } + + GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + CameraHardware::CameraModel camera_model = + camera.camera_hardware_.camera_model_; + + // Get the calibration values from the camera + + cv::Mat cameracalibrationMatrix_ = camera.camera_hardware_.calibrationMatrix_; + cv::Mat cameraDistortionVector_ = + camera.camera_hardware_.cameraDistortionVector_; + + cv::Mat unDistortedBall1Img; + cv::Mat m_undistMap1, m_undistMap2; + + if (camera.camera_hardware_.camera_is_mono()) { + cv::initUndistortRectifyMap( + cameracalibrationMatrix_, cameraDistortionVector_, cv::Mat(), + cameracalibrationMatrix_, cv::Size(img.cols, img.rows), CV_8UC1, + m_undistMap1, m_undistMap2); + } else { + cv::initUndistortRectifyMap( + cameracalibrationMatrix_, cameraDistortionVector_, cv::Mat(), + cameracalibrationMatrix_, cv::Size(img.cols, img.rows), CV_32FC1, + m_undistMap1, m_undistMap2); + } + + cv::remap(img, unDistortedBall1Img, m_undistMap1, m_undistMap2, + cv::INTER_LINEAR); + + return unDistortedBall1Img; +} - // Using a single pi requires both cameras to be connected to that Pi. - // If we are not receiving two sets of camera data, then something is wrong. - int new_line_position = line.find('\n'); +bool ConfigCameraForFullScreenWatching(const GolfSimCamera &c) { - if (new_line_position == (int)string::npos) { + if (lci::camera_crop_configuration_ == lci::kFullScreen) { + // This takes time, so no need to do it repeatedly if not necessary + // the flag will be reset if/when a cropped mode is setup + GS_LOG_MSG(trace, "ConfigCameraForFullScreenWatching already configured."); + return true; + } - // There is only one line of discovered information + uint width = c.camera_hardware_.resolution_x_; + uint height = c.camera_hardware_.resolution_y_; - if (GolfSimOptions::GetCommandLineOptions().run_single_pi_) { - GS_LOG_TRACE_MSG(error, "No expected new line found in camera location output. Missing camera when running in single-pi mode."); - return false; - } - else { - // We have only a single line of information. Do not need to do anything else - } - } - else { - // Assume (TBD - Confirm with Pi people) that the camera on camera unit 0 - // (the port nearest the LAN port) will correspond to the first line of the - // returned media-ctl output - - if (camera_number == GsCameraNumber::kGsCamera1) { - // Gee the information from the first line - std::string first_line_str = line.substr(0, new_line_position); - line = first_line_str; - } - else { - // Gee the information from the second line - std::string first_line_str = line.substr(new_line_position + 1); - line = first_line_str; - } - } + if (width <= 0 || height <= 0) { + GS_LOG_MSG(error, "ConfigCameraForFullScreenWatching called with camera " + "that has no resolution set."); + return false; + } - // Parse out the media and device numbers from what should be the first line of the media-ctl location report + // Ensure no cropping and full resolution on the camera + std::string mediaCtlCmd = GetCmdLineForMediaCtlCropping( + c, cv::Vec2i(width, height), cv::Vec2i(0, 0)); + int cmdResult = system(mediaCtlCmd.c_str()); - int last_space_position = line.rfind(' '); + if (cmdResult != 0) { + GS_LOG_MSG(error, "system(mediaCtlCmd) failed."); + return false; + } - std::string device_number_str; + LibCameraInterface::camera_crop_configuration_ = + LibCameraInterface::kFullScreen; - if (last_space_position != (int)string::npos) { - device_number_str = line.substr(last_space_position + 1); - } - else { - GS_LOG_TRACE_MSG(error, "No space found"); - return false; - } + return true; +} - int first_space_position = line.find(' '); +bool SetLibcameraTuningFileEnvVariable(const GolfSimCamera &camera) { - std::string media_number_str; + GolfSimConfiguration::PiModel pi_model = GolfSimConfiguration::GetPiModel(); - if (first_space_position != (int)string::npos) { - media_number_str = line.substr(0, first_space_position); - } - else { - GS_LOG_TRACE_MSG(error, "No space found"); - return false; - } + std::string tuning_file; - if (media_number_str.empty() || device_number_str.empty()) { - GS_LOG_TRACE_MSG(error, "Failed to parse media and device number strings"); - return false; - } + if (camera.camera_hardware_.camera_is_mono()) { + // If this is a mono camera, than we must use the "mono" tuning file, + // regardless of whether the camera is camera 1 or camera 2 - media_number = std::stoi(media_number_str); - device_number = std::stoi(device_number_str); - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); - return false; + if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { + tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296_mono.json"; + } else { + tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296_mono.json"; } + } else if (GolfSimOptions::GetCommandLineOptions().GetCameraNumber() == + GsCameraNumber::kGsCamera1) { - // Signal that we won't need to do this again during this run. - camera_location_found_ = true; - previously_found_media_number_ = media_number; - previously_found_device_number_ = device_number; - - return true; -} - - -bool SendCameraCroppingCommand(const GolfSimCamera& camera, cv::Vec2i& cropping_window_size, cv::Vec2i& cropping_window_offset) { - - std::string mediaCtlCmd = GetCmdLineForMediaCtlCropping(camera, cropping_window_size, cropping_window_offset); - int cmdResult = system(mediaCtlCmd.c_str()); - - if (cmdResult != 0) { - GS_LOG_TRACE_MSG(error, "system(mediaCtlCmd) failed."); - return false; + if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { + tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296.json"; + } else { + tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296.json"; } - return true; -} - - -bool ConfigurePostProcessing(const cv::Vec2i& roi_size, const cv::Vec2i& roi_offset ) { + } else { + // Use the infrared (noir) tuning file + if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { + tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296_noir.json"; + } else { + tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296_noir.json"; + } + } - float kDifferenceM = 0.; - float kDifferenceC = 0.; - float kRegionThreshold = 0.; - float kMaxRegionThreshold = 0.; - uint kFramePeriod = 0; - uint kHSkip = 0; - uint kVSkip = 0; + setenv("LIBCAMERA_RPI_TUNING_FILE", tuning_file.c_str(), 1); + return true; +} - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kDifferenceM", kDifferenceM); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kDifferenceC", kDifferenceC); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kRegionThreshold", kRegionThreshold); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kMaxRegionThreshold", kMaxRegionThreshold); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kFramePeriod", kFramePeriod); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kHSkip", kHSkip); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kVSkip", kVSkip); +LibcameraJpegApp *ConfigureForLibcameraStill(const GolfSimCamera &camera) { - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kCroppedImagePixelOffsetLeft", kCroppedImagePixelOffsetLeft); - GolfSimConfiguration::SetConstant("gs_config.motion_detect_stage.kCroppedImagePixelOffsetUp", kCroppedImagePixelOffsetUp); + const GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + int hardware_camera_index = camera_number; - // These values will be used within the motion-detect post-processing + // Check first if we are already setup and can skip this - MotionDetectStage::incoming_configuration.use_incoming_configuration = true; // Don't use .json file values -- use the following + LibcameraJpegApp *app = lci::libcamera_app_[hardware_camera_index]; - - MotionDetectStage::incoming_configuration.roi_x = roi_offset[0]; - MotionDetectStage::incoming_configuration.roi_y = roi_offset[1]; + if (app != nullptr || lci::libcamera_configuration_[hardware_camera_index] == + lci::CameraConfiguration::kStillPicture) { + return lci::libcamera_app_[camera_number]; + } - MotionDetectStage::incoming_configuration.roi_width = roi_size[0]; - MotionDetectStage::incoming_configuration.roi_height = roi_size[1]; + if (app != nullptr) { + // The camera is configured, but just not for the right purposes + // Deconfigure and then re-configure - MotionDetectStage::incoming_configuration.difference_m = kDifferenceM; - MotionDetectStage::incoming_configuration.difference_c = kDifferenceC; - MotionDetectStage::incoming_configuration.region_threshold = kRegionThreshold; - MotionDetectStage::incoming_configuration.max_region_threshold = kMaxRegionThreshold; - MotionDetectStage::incoming_configuration.frame_period = kFramePeriod; - MotionDetectStage::incoming_configuration.hskip = kHSkip; // TBD - don't hard code the skip factor - MotionDetectStage::incoming_configuration.vskip = kVSkip; - MotionDetectStage::incoming_configuration.verbose = 2; - MotionDetectStage::incoming_configuration.showroi = true; + if (!DeConfigureForLibcameraStill(camera_number)) { + GS_LOG_TRACE_MSG(error, "failed to DeConfigureForLibcameraStill."); + return nullptr; + } + } - return true; -} + // At this point, we know that we actually have to (re)configure the camera + app = new LibcameraJpegApp; -bool ConfigureLibCameraOptions(const GolfSimCamera& camera, RPiCamEncoder& app, const cv::Vec2i& cropping_window_size, uint cropped_frame_rate_fps) { + if (app == nullptr) { + GS_LOG_TRACE_MSG(error, "failed to create a new LibcameraJpegApp."); + return nullptr; + } - GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + // Make sure we save the new app for later + lci::libcamera_app_[hardware_camera_index] = app; - VideoOptions* options = app.GetOptions(); + try { + StillOptions *options = app->GetOptions(); char dummy_arguments[] = "DummyExecutableName"; - char* argv[] = { dummy_arguments, NULL }; + char *argv[] = {dummy_arguments, NULL}; - if (!options->Parse(1, argv)) - { - GS_LOG_TRACE_MSG(trace, "failed to parse dummy command line."); - return false; + if (!options->Parse(1, argv)) { + GS_LOG_TRACE_MSG(error, "failed to parse dummy command line."); + return nullptr; } SetLibCameraLoggingOff(); - options->no_raw = true; // See https://forums.raspberrypi.com/viewtopic.php?t=369927 - cameras won't work unless this is set. - - std::string shutter_speed_string; - // Generally need to crank up gain due to short exposure time at high FPS. - float camera_gain = 0.0; - - if (GolfSimClubData::kGatherClubData) { - camera_gain = GolfSimClubData::kClubImageCameraGain; - shutter_speed_string = std::to_string((int)(GolfSimClubData::kClubImageShutterSpeedMultiplier * (1. / cropped_frame_rate_fps * 1000000.))) + "us"; // TBD - should be 1,000,000 for uS setting + double camera_gain = 1.0; + double camera_contrast = 1.0; + long still_shutter_time_uS = 10000; + + if (GolfSimOptions::GetCommandLineOptions().GetCameraNumber() == + GsCameraNumber::kGsCamera1) { + camera_gain = LibCameraInterface::kCamera1Gain; + camera_contrast = LibCameraInterface::kCamera1Contrast; + still_shutter_time_uS = LibCameraInterface::kCamera1StillShutterTimeuS; + } else { + // We're working with camera 2, which doesn't normally take still + // pictures. BUT, we might be doing a calibration shot, and if so, we'll + // want to adjust the gain & contrast to a lower value because of the + // brighter (non-strobed) environment + + if (!GolfSimOptions::GetCommandLineOptions().lm_comparison_mode_) { + camera_gain = LibCameraInterface::kCamera2Gain; + } else { + camera_gain = LibCameraInterface::kCamera2ComparisonGain; + } + + camera_contrast = LibCameraInterface::kCamera2Contrast; + + // TBD - This code seems backward. But everything is working right now, + // so let's not change until we can really test it all. + if (GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2Calibrate || + GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2OnePulseOnly || + GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2BallLocation || + GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2AutoCalibrate) { + + camera_gain = LibCameraInterface::kCamera2CalibrateOrLocationGain; + still_shutter_time_uS = LibCameraInterface::kCamera2StillShutterTimeuS; + } else { + still_shutter_time_uS = + 6 * LibCameraInterface::kCamera2StillShutterTimeuS; + } } - else { - camera_gain = LibCameraInterface::kCamera1HighFPSGain; - shutter_speed_string = std::to_string((int)(1. / cropped_frame_rate_fps * 1000000.)) + "us"; // TBD - should be 1,000,000 for uS setting - } - - options->gain = camera_gain; - options->shutter.set(shutter_speed_string); // TBD - should be 1,000,000 for uS setting - options->saturation = (camera_number == GsCameraNumber::kGsCamera1) ? LibCameraInterface::kCamera1Saturation : LibCameraInterface::kCamera2Saturation; + // Assume camera 1 will be at slot 0 in all cases. Camera 2 will be at slot + // 1 only in a single Pi system. + options->camera = (camera_number == GsCameraNumber::kGsCamera1 || + !GolfSimOptions::GetCommandLineOptions().run_single_pi_) + ? 0 + : 1; - GS_LOG_MSG(trace, "Saturation = " + std::to_string(options->saturation)); - - options->timeout.set("0ms"); - - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot1CameraType; + // Shouldn't need a special gain to take a "normal" picture. Default will + // be 1.0 from the command line options. + options->gain = camera_gain; + options->saturation = (camera_number == GsCameraNumber::kGsCamera1) + ? LibCameraInterface::kCamera1Saturation + : LibCameraInterface::kCamera2Saturation; + options->contrast = camera_contrast; + options->timeout.set("5s"); + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot1CameraType; if (camera_model != CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { - options->denoise = "cdn_off"; - } - else { - options->denoise = "auto"; + options->denoise = "cdn_off"; + } else { + options->denoise = "auto"; } - GS_LOG_TRACE_MSG(trace, "Camera denoise option set to: " + options->denoise); - options->framerate = cropped_frame_rate_fps; + GS_LOG_TRACE_MSG(trace, + "Camera denoise option set to: " + options->denoise); + options->immediate = true; // TBD - Trying this for now. May have to work + // on white balance too + options->awb = "indoor"; // TBD - Trying this for now. May have to work on + // white balance too options->nopreview = true; - options->lores_width = 0; - options->lores_height = 0; options->viewfinder_width = 0; options->viewfinder_height = 0; + options->shutter.set(std::to_string(still_shutter_time_uS) + "us"); + if (GolfSimOptions::GetCommandLineOptions().use_non_IR_camera_) { + options->shutter.set("12000us"); // uS + } options->info_text = ""; - options->level = "4.2"; - const CameraHardware::CameraOrientation camera_orientation = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraOrientation : GolfSimCamera::kSystemSlot2CameraOrientation; + const CameraHardware::CameraOrientation camera_orientation = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraOrientation + : GolfSimCamera::kSystemSlot2CameraOrientation; if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { - // Tell libcamera to flip the image vertically back to where it should be - options->transform = libcamera::Transform::VFlip; - GS_LOG_MSG(trace, "Flipping still picture upside down."); - } - else { - GS_LOG_MSG(trace, "NOT flipping still picture upside down."); + // Tell libcamera to flip the image vertically back to where it should be + options->transform = libcamera::Transform::VFlip; + GS_LOG_MSG(trace, "Flipping still picture upside down."); + } else { + GS_LOG_MSG(trace, "NOT flipping still picture upside down."); } - // On the Pi5, there's no hardware H.264 encoding, so let's try to turn it off entirely - // TBD - See video_options.cpp to consider other options like libav - options->codec = "yuv420"; // was h.264, but that no longer works on Pi5 - - if (GolfSimConfiguration::GetPiModel() == GolfSimConfiguration::PiModel::kRPi5) { - options->tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296.json"; - } - else { - options->tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296.json"; - } - setenv("LIBCAMERA_RPI_TUNING_FILE", options->tuning_file.c_str(), 1); - - // TBD - We are switching away from having the post_process_file trigger the - // dynamic loading of the motion_detection module. Instead, hop[efully for speed, - // we will use a statically-bound motion_detection module. See ball_watcher.cpp - // options->post_process_file = LibCameraInterface::kCameraMotionDetectSettings; - - if (cropping_window_size[0] > 0 && cropping_window_size[1] > 0) { - options->width = cropping_window_size[0]; - options->height = cropping_window_size[1]; + if (!SetLibcameraTuningFileEnvVariable(camera)) { + GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); + return nullptr; } if (options->verbose >= 2) - options->Print(); - - return true; -} - - -// For example, to set the GS cam back to its default, use "(0, 0)/1456x1088" -// 98x88 can deliver 572 FPS on the GS cam. -std::string GetCmdLineForMediaCtlCropping(const GolfSimCamera &camera, cv::Vec2i croppedHW, cv::Vec2i crop_offset_xY) { + options->Print(); - std::string s; + app->OpenCamera(); + // The RGB flag still works for grayscale mono images + uint flags = RPiCamApp::FLAG_STILL_RGB; + app->ConfigureStill(flags); - int media_number = -1; - int device_number = -1; + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR in ConfigureForLibcameraStill: *** " + + std::string(e.what()) + " ***"); + return nullptr; + } - if (!DiscoverCameraLocation(camera.camera_hardware_.camera_number_, media_number, device_number)) { - GS_LOG_MSG(error, "Could not DiscoverCameraLocation"); - return ""; - } - - // The format will be different for mono cameras amd cp;pr - std::string format = (camera.camera_hardware_.camera_is_mono()) ? "Y10_1X10" : "SBGGR10_1X10"; - - s += "#!/bin/sh\n"; - s += "if media-ctl -d \"/dev/media" + std::to_string(media_number) + "\" --set-v4l2 \"'imx296 " + std::to_string(device_number) + "-001a':0 [fmt:" + format + "/" + std::to_string(croppedHW[0]) + "x" + std::to_string(croppedHW[1]) + " crop:(" + std::to_string(crop_offset_xY[0]) + "," + std::to_string(crop_offset_xY[1]) + ")/" + - std::to_string(croppedHW[0]) + "x" + std::to_string(croppedHW[1]) + "]\" > /dev/null; then echo -e \"/dev/media" + std::to_string(media_number) + "\" > /dev/null; break; fi\n"; + // Note the type of configuration we've done + lci::libcamera_configuration_[hardware_camera_index] = + lci::CameraConfiguration::kStillPicture; - return s; + return app; } +bool DeConfigureForLibcameraStill(const GsCameraNumber camera_number) { -bool RetrieveCameraInfo(const GsCameraNumber camera_number, cv::Vec2i& resolution, uint& frameRate, bool restartCamera) { - - LibcameraJpegApp app; - - if (restartCamera) { - - try - { - StillOptions* options = app.GetOptions(); - - char dummy_arguments[] = "DummyExecutableName"; - char* argv[] = { dummy_arguments, NULL }; - - if (options->Parse(1, argv)) - { - if (options->verbose >= 2) - options->Print(); - - SetLibCameraLoggingOff(); - - options->no_raw = true; // See https://forums.raspberrypi.com/viewtopic.php?t=369927 - - // Set the camera number (0 or 1, likely) when we have more than one camera - options->camera = (camera_number == GsCameraNumber::kGsCamera1 || !GolfSimOptions::GetCommandLineOptions().run_single_pi_) ? 0 : 1; - - // Get the camera open for a moment so that we can read its settings - app.OpenCamera(); - // GS_LOG_TRACE_MSG(trace, "About to ConfigureViewfinder"); - app.ConfigureViewfinder(); - // GS_LOG_TRACE_MSG(trace, "About to StartCamera"); - app.StartCamera(); - // GS_LOG_TRACE_MSG(trace, "About to StopCamera"); - app.StopCamera(); - } - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); - return false; - } - } - - std::vector> cameras = app.GetCameras(); - - if (cameras.size() == 0) - { - GS_LOG_MSG(error, "Could not getCameras."); - return false; - } - - int camera_slot_number = (camera_number == GsCameraNumber::kGsCamera1 || !GolfSimOptions::GetCommandLineOptions().run_single_pi_) ? 0 : 1; - - auto const& cam = cameras[camera_slot_number]; - - std::unique_ptr config = cam->generateConfiguration({ libcamera::StreamRole::Raw }); - if (!config) - { - GS_LOG_MSG(error, "Could not get cam config."); - return false; - } - - // Parse out the frames-per-second value (FPS) - auto fd_ctrl = cam->controls().find(&controls::FrameDurationLimits); - auto crop_ctrl = cam->properties().get(properties::ScalerCropMaximum); - libcamera::Rectangle cropRect = crop_ctrl.value(); - double fps = fd_ctrl == cam->controls().end() ? NAN : (1e6 / fd_ctrl->second.min().get()); - std::cout << std::fixed << std::setprecision(2) << "[" - << fps << " fps - " << crop_ctrl->toString() << " crop" << "]\n"; - - // Return results - resolution = cv::Vec2i(cropRect.width, cropRect.height); - frameRate = fps; - - return true; + if (lci::libcamera_app_[camera_number] == nullptr) { + GS_LOG_TRACE_MSG(error, "DeConfigureForLibcameraStill called, but " + "camera_app was null. Doing nothing."); + return false; + } + + if (lci::libcamera_configuration_[camera_number] != + lci::CameraConfiguration::kStillPicture) { + GS_LOG_TRACE_MSG( + warning, + "DeConfigureForLibcameraStill called, but camera_app was in the wrong " + "configurations (configure was mis-matched). Camera_number was: " + + std::to_string((int)lci::libcamera_configuration_[camera_number]) + + " Ignoring."); + } + + try { + lci::libcamera_app_[camera_number]->StopCamera(); + lci::libcamera_app_[camera_number]->Teardown(); // TBD - Need? + delete lci::libcamera_app_[camera_number]; + lci::libcamera_app_[camera_number] = nullptr; + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR in DeConfigureForLibcameraStill: *** " + + std::string(e.what()) + " ***"); + return false; + } + + lci::libcamera_configuration_[camera_number] = + lci::CameraConfiguration::kNotConfigured; + + return true; } +// Actually from libcamera_jpeg code, not libcamera_still +bool TakeLibcameraStill(const GolfSimCamera &camera, cv::Mat &img) { + LibcameraJpegApp *app = ConfigureForLibcameraStill(camera); -cv::Mat LibCameraInterface::undistort_camera_image(const cv::Mat& img, const GolfSimCamera& camera) { - - if (!camera.camera_hardware_.use_undistortion_matrix_) { - GS_LOG_MSG(trace, "undistort_camera_image ignoring camera with no undistortion matrix. Returning original image."); - return img; - } - - GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; - CameraHardware::CameraModel camera_model = camera.camera_hardware_.camera_model_; - - // Get the calibration values from the camera - - cv::Mat cameracalibrationMatrix_ = camera.camera_hardware_.calibrationMatrix_; - cv::Mat cameraDistortionVector_ = camera.camera_hardware_.cameraDistortionVector_; - - cv::Mat unDistortedBall1Img; - cv::Mat m_undistMap1, m_undistMap2; + if (app == nullptr) { + GS_LOG_TRACE_MSG(error, "failed to ConfigureForLibcameraStill."); + return false; + } - if (camera.camera_hardware_.camera_is_mono()) { - cv::initUndistortRectifyMap(cameracalibrationMatrix_, cameraDistortionVector_, cv::Mat(), cameracalibrationMatrix_, cv::Size(img.cols, img.rows), CV_8UC1, m_undistMap1, m_undistMap2); - } - else { - cv::initUndistortRectifyMap(cameracalibrationMatrix_, cameraDistortionVector_, cv::Mat(), cameracalibrationMatrix_, cv::Size(img.cols, img.rows), CV_32FC1, m_undistMap1, m_undistMap2); - } + try { + still_image_event_loop(*app, img); + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + return false; + } - cv::remap(img, unDistortedBall1Img, m_undistMap1, m_undistMap2, cv::INTER_LINEAR); + if (!DeConfigureForLibcameraStill( + GolfSimOptions::GetCommandLineOptions().GetCameraNumber())) { + GS_LOG_TRACE_MSG(error, "failed to DeConfigureForLibcameraStill."); + return false; + } - return unDistortedBall1Img; + return true; } +// TBD - This really seems like it should exist in the gs_camera module? +bool TakeRawPicture(const GolfSimCamera &camera, cv::Mat &img) { -bool ConfigCameraForFullScreenWatching(const GolfSimCamera& c) { + const GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; - if (lci::camera_crop_configuration_ == lci::kFullScreen) { - // This takes time, so no need to do it repeatedly if not necessary - // the flag will be reset if/when a cropped mode is setup - GS_LOG_MSG(trace, "ConfigCameraForFullScreenWatching already configured."); - return true; - } + const CameraHardware::CameraModel camera_model = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraType + : GolfSimCamera::kSystemSlot2CameraType; - uint width = c.camera_hardware_.resolution_x_; - uint height = c.camera_hardware_.resolution_y_; + // Ensure we have full resolution + ConfigCameraForFullScreenWatching(camera); - if (width <= 0 || height <= 0) { - GS_LOG_MSG(error, "ConfigCameraForFullScreenWatching called with camera that has no resolution set."); - return false; - } + cv::Mat initialImg; + if (!TakeLibcameraStill(camera, initialImg)) { + GS_LOG_MSG(error, "Failed to take still picture."); + return false; + } - // Ensure no cropping and full resolution on the camera - std::string mediaCtlCmd = GetCmdLineForMediaCtlCropping(c, cv::Vec2i(width, height), cv::Vec2i(0, 0)); - int cmdResult = system(mediaCtlCmd.c_str()); + if (initialImg.empty()) { + return false; + } - if (cmdResult != 0) { - GS_LOG_MSG(error, "system(mediaCtlCmd) failed."); - return false; - } + img = + golf_sim::LibCameraInterface::undistort_camera_image(initialImg, camera); - LibCameraInterface::camera_crop_configuration_ = LibCameraInterface::kFullScreen; - - return true; + return true; } - - -bool SetLibcameraTuningFileEnvVariable(const GolfSimCamera& camera) { - - GolfSimConfiguration::PiModel pi_model = GolfSimConfiguration::GetPiModel(); - - std::string tuning_file; - - if (camera.camera_hardware_.camera_is_mono()) { - // If this is a mono camera, than we must use the "mono" tuning file, regardless of whether - // the camera is camera 1 or camera 2 - - if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { - tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296_mono.json"; - } - else { - tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296_mono.json"; - } - } - else if (GolfSimOptions::GetCommandLineOptions().GetCameraNumber() == GsCameraNumber::kGsCamera1) { - - if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { - tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296.json"; - } - else { - tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296.json"; - } - } - else { - // Use the infrared (noir) tuning file - if (pi_model == GolfSimConfiguration::PiModel::kRPi5) { - tuning_file = "/usr/share/libcamera/ipa/rpi/pisp/imx296_noir.json"; - } - else { - tuning_file = "/usr/share/libcamera/ipa/rpi/vc4/imx296_noir.json"; - } +// Enhanced ball detection using YOLO when configured +bool CheckForBallEnhanced(GolfBall &ball, cv::Mat &img) { + bool use_yolo = (golf_sim::BallImageProc::kBallPlacementDetectionMethod == + "experimental"); + + GsCameraNumber camera_number = + GolfSimOptions::GetCommandLineOptions().GetCameraNumber(); + const CameraHardware::CameraModel camera_model = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraType + : GolfSimCamera::kSystemSlot2CameraType; + const CameraHardware::LensType camera_lens_type = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1LensType + : GolfSimCamera::kSystemSlot2LensType; + const CameraHardware::CameraOrientation camera_orientation = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraOrientation + : GolfSimCamera::kSystemSlot2CameraOrientation; + + GolfSimCamera camera; + camera.camera_hardware_.init_camera_parameters( + camera_number, camera_model, camera_lens_type, camera_orientation); + + if (!TakeRawPicture(camera, img)) { + GS_LOG_MSG(error, "Failed to TakeRawPicture."); + return false; + } + + cv::Vec2i search_center = camera.GetExpectedBallCenter(); + + if (use_yolo) { + if (!golf_sim::BallImageProc::PreloadYOLOModel()) { + GS_LOG_MSG(warning, "YOLO model not available, using legacy detection"); + } else { + std::vector detected_circles; + bool detected = golf_sim::BallImageProc::DetectBallsONNX( + img, golf_sim::BallImageProc::BallSearchMode::kFindPlacedBall, + detected_circles); + + if (detected && !detected_circles.empty()) { + GsCircle best_circle; + float best_distance = FLT_MAX; + + for (const auto &circle : detected_circles) { + float dx = circle[0] - search_center[0]; + float dy = circle[1] - search_center[1]; + float distance = sqrt(dx * dx + dy * dy); + + if (distance < best_distance) { + best_distance = distance; + best_circle = circle; + } + } + + if (best_distance < 200) { + ball.ball_circle_ = best_circle; + ball.measured_radius_pixels_ = best_circle[2]; + ball.search_area_center_ = search_center; + ball.search_area_radius_ = 200; + + return true; + } + } } + } - setenv("LIBCAMERA_RPI_TUNING_FILE", tuning_file.c_str(), 1); - - return true; + bool expectBall = false; + return camera.GetCalibratedBall(camera, img, ball, search_center, expectBall); } +// TBD - This really seems like it should exist in the gs_camera module? +bool CheckForBall(GolfBall &ball, cv::Mat &img) { + return CheckForBallEnhanced(ball, img); +} -LibcameraJpegApp* ConfigureForLibcameraStill(const GolfSimCamera& camera) { - - const GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; - int hardware_camera_index = camera_number; - - // Check first if we are already setup and can skip this +bool CheckForBallLegacy(GolfBall &ball, cv::Mat &img) { + + GsCameraNumber camera_number = + GolfSimOptions::GetCommandLineOptions().GetCameraNumber(); + + // Figure out where the ball is + // TBD - This repeats the camera initialization that we just did + const CameraHardware::CameraModel camera_model = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraType + : GolfSimCamera::kSystemSlot2CameraType; + const CameraHardware::LensType camera_lens_type = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1LensType + : GolfSimCamera::kSystemSlot2LensType; + const CameraHardware::CameraOrientation camera_orientation = + (camera_number == GsCameraNumber::kGsCamera1) + ? GolfSimCamera::kSystemSlot1CameraOrientation + : GolfSimCamera::kSystemSlot2CameraOrientation; + + GolfSimCamera camera; + camera.camera_hardware_.init_camera_parameters( + camera_number, camera_model, camera_lens_type, camera_orientation); + camera.camera_hardware_.firstCannedImageFileName = + std::string("/mnt/VerdantShare/dev/GolfSim/LM/Images/") + + "FirstWaitingImage"; + camera.camera_hardware_.firstCannedImage = img; + + // If we are checking to see if a ball + if (!TakeRawPicture(camera, img)) { + GS_LOG_MSG(error, "Failed to TakeRawPicture."); + return false; + } + + cv::Vec2i search_area_center = camera.GetExpectedBallCenter(); + + bool expectBall = false; + bool success = camera.GetCalibratedBall(camera, img, ball, search_area_center, + expectBall); + + if (!success) { + return false; + } + + return true; +} - LibcameraJpegApp* app = lci::libcamera_app_[hardware_camera_index]; +// The following code is only relevant to the camera 2 system +bool WaitForCam2Trigger(cv::Mat &return_image) { + LibcameraJpegApp app; - if (app != nullptr || lci::libcamera_configuration_[hardware_camera_index] == lci::CameraConfiguration::kStillPicture) { - return lci::libcamera_app_[camera_number]; - } + cv::Mat raw_image; - if (app != nullptr) { - // The camera is configured, but just not for the right purposes - // Deconfigure and then re-configure + // Create a camera just to set the resolution and for un-distort operation + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot2CameraType; + const CameraHardware::LensType camera_lens_type = + GolfSimCamera::kSystemSlot2LensType; + const CameraHardware::CameraOrientation camera_orientation = + GolfSimCamera::kSystemSlot2CameraOrientation; - if (!DeConfigureForLibcameraStill(camera_number)) { - GS_LOG_TRACE_MSG(error, "failed to DeConfigureForLibcameraStill."); - return nullptr; - } - } + GolfSimCamera c; + c.camera_hardware_.init_camera_parameters(GsCameraNumber::kGsCamera2, + camera_model, camera_lens_type, + camera_orientation); - // At this point, we know that we actually have to (re)configure the camera + try { + StillOptions *options = app.GetOptions(); - app = new LibcameraJpegApp; + char dummy_arguments[] = "DummyExecutableName"; + char *argv[] = {dummy_arguments, NULL}; - if (app == nullptr) { - GS_LOG_TRACE_MSG(error, "failed to create a new LibcameraJpegApp."); - return nullptr; + if (!options->Parse(1, argv)) { + return -1; } - // Make sure we save the new app for later - lci::libcamera_app_[hardware_camera_index] = app; - - try - { - StillOptions* options = app->GetOptions(); - - char dummy_arguments[] = "DummyExecutableName"; - char* argv[] = { dummy_arguments, NULL }; - - if (!options->Parse(1, argv)) - { - GS_LOG_TRACE_MSG(error, "failed to parse dummy command line."); - return nullptr; - } - - SetLibCameraLoggingOff(); - - double camera_gain = 1.0; - double camera_contrast = 1.0; - long still_shutter_time_uS = 10000; - - if (GolfSimOptions::GetCommandLineOptions().GetCameraNumber() == GsCameraNumber::kGsCamera1) { - camera_gain = LibCameraInterface::kCamera1Gain; - camera_contrast = LibCameraInterface::kCamera1Contrast; - still_shutter_time_uS = LibCameraInterface::kCamera1StillShutterTimeuS; - } - else { - // We're working with camera 2, which doesn't normally take still pictures. - // BUT, we might be doing a calibration shot, - // and if so, we'll want to adjust the gain & contrast to a lower value because of the - // brighter (non-strobed) environment - - if (!GolfSimOptions::GetCommandLineOptions().lm_comparison_mode_) { - camera_gain = LibCameraInterface::kCamera2Gain; - } - else { - camera_gain = LibCameraInterface::kCamera2ComparisonGain; - } - - camera_contrast = LibCameraInterface::kCamera2Contrast; - - // TBD - This code seems backward. But everything is working right now, - // so let's not change until we can really test it all. - if (GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2Calibrate || - GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2OnePulseOnly || - GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2BallLocation || - GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2AutoCalibrate) { - - camera_gain = LibCameraInterface::kCamera2CalibrateOrLocationGain; - still_shutter_time_uS = LibCameraInterface::kCamera2StillShutterTimeuS; - } - else { - still_shutter_time_uS = 6 * LibCameraInterface::kCamera2StillShutterTimeuS; - } - } - - // Assume camera 1 will be at slot 0 in all cases. Camera 2 will be at slot 1 - // only in a single Pi system. - options->camera = (camera_number == GsCameraNumber::kGsCamera1 || !GolfSimOptions::GetCommandLineOptions().run_single_pi_) ? 0 : 1; - - // Shouldn't need a special gain to take a "normal" picture. Default will be 1.0 - // from the command line options. - options->gain = camera_gain; - options->saturation = (camera_number == GsCameraNumber::kGsCamera1) ? LibCameraInterface::kCamera1Saturation : LibCameraInterface::kCamera2Saturation; - options->contrast = camera_contrast; - options->timeout.set("5s"); - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot1CameraType; - if (camera_model != CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { - options->denoise = "cdn_off"; - } - else { - options->denoise = "auto"; - } - - GS_LOG_TRACE_MSG(trace, "Camera denoise option set to: " + options->denoise); - options->immediate = true; // TBD - Trying this for now. May have to work on white balance too - options->awb = "indoor"; // TBD - Trying this for now. May have to work on white balance too - options->nopreview = true; - options->viewfinder_width = 0; - options->viewfinder_height = 0; - options->shutter.set(std::to_string(still_shutter_time_uS) + "us"); - if (GolfSimOptions::GetCommandLineOptions().use_non_IR_camera_) { - options->shutter.set("12000us"); // uS - } - options->info_text = ""; - - const CameraHardware::CameraOrientation camera_orientation = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraOrientation : GolfSimCamera::kSystemSlot2CameraOrientation; - - if (camera_orientation == CameraHardware::CameraOrientation::kUpsideDown) { - // Tell libcamera to flip the image vertically back to where it should be - options->transform = libcamera::Transform::VFlip; - GS_LOG_MSG(trace, "Flipping still picture upside down."); - } - else { - GS_LOG_MSG(trace, "NOT flipping still picture upside down."); - } - - if (!SetLibcameraTuningFileEnvVariable(camera) ) { - GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); - return nullptr; - } - - if (options->verbose >= 2) - options->Print(); - - app->OpenCamera(); - // The RGB flag still works for grayscale mono images - uint flags = RPiCamApp::FLAG_STILL_RGB; - app->ConfigureStill(flags); + SetLibCameraLoggingOff(); + // On a two-Pi system, each Pi has just one camera, and that camera will be + // in slot 0 On a single-pi system the one Pi 5 has both cameras. And + // Camera 2 will be in slot 1 because Camera 1 is in slot 0. + if (GolfSimOptions::GetCommandLineOptions().run_single_pi_) { + options->camera = 1; + } else { + options->camera = 0; } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR in ConfigureForLibcameraStill: *** " + std::string(e.what()) + " ***"); - return nullptr; - } - - // Note the type of configuration we've done - lci::libcamera_configuration_[hardware_camera_index] = lci::CameraConfiguration::kStillPicture; - - return app; -} - - -bool DeConfigureForLibcameraStill(const GsCameraNumber camera_number) { - - if (lci::libcamera_app_[camera_number] == nullptr) { - GS_LOG_TRACE_MSG(error, "DeConfigureForLibcameraStill called, but camera_app was null. Doing nothing."); - return false; + if (GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2Calibrate || + GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2BallLocation || + GolfSimOptions::GetCommandLineOptions().system_mode_ == + SystemMode::kCamera2AutoCalibrate) { + + options->gain = LibCameraInterface::kCamera2CalibrateOrLocationGain; + options->saturation = (float)LibCameraInterface::kCamera2Saturation; + + } else if (GolfSimClubs::GetCurrentClubType() == GolfSimClubs::kPutter) { + options->gain = LibCameraInterface::kCamera2PuttingGain; + options->contrast = LibCameraInterface::kCamera2PuttingContrast; + } else { + if (!GolfSimOptions::GetCommandLineOptions().lm_comparison_mode_) { + options->gain = LibCameraInterface::kCamera2Gain; + options->saturation = (float)LibCameraInterface::kCamera2Saturation; + } else { + options->gain = LibCameraInterface::kCamera2ComparisonGain; + options->saturation = (float)LibCameraInterface::kCamera2Saturation; + } + + options->contrast = LibCameraInterface::kCamera2Contrast; } - if (lci::libcamera_configuration_[camera_number] != lci::CameraConfiguration::kStillPicture) { - GS_LOG_TRACE_MSG(warning, "DeConfigureForLibcameraStill called, but camera_app was in the wrong configurations (configure was mis-matched). Camera_number was: " + std::to_string((int)lci::libcamera_configuration_[camera_number]) + " Ignoring."); - } + options->saturation = LibCameraInterface::kCamera2Saturation; + GS_LOG_MSG(trace, "Flight camera saturation = " + + std::to_string(options->saturation)); - try - { - lci::libcamera_app_[camera_number]->StopCamera(); - lci::libcamera_app_[camera_number]->Teardown(); // TBD - Need? - delete lci::libcamera_app_[camera_number]; - lci::libcamera_app_[camera_number] = nullptr; - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR in DeConfigureForLibcameraStill: *** " + std::string(e.what()) + " ***"); - return false; + options->immediate = true; + options->timeout.set("0ms"); // Wait forever for external trigger + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot1CameraType; + if (camera_model != CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { + options->denoise = "cdn_off"; + } else { + options->denoise = "auto"; } - lci::libcamera_configuration_[camera_number] = lci::CameraConfiguration::kNotConfigured; - - return true; -} - - - -// Actually from libcamera_jpeg code, not libcamera_still -bool TakeLibcameraStill(const GolfSimCamera &camera, cv::Mat& img) { - - LibcameraJpegApp *app = ConfigureForLibcameraStill(camera); - - if (app == nullptr) { - GS_LOG_TRACE_MSG(error, "failed to ConfigureForLibcameraStill."); - return false; - } + GS_LOG_TRACE_MSG(trace, + "Camera denoise option set to: " + options->denoise); + options->nopreview = true; + // TBD - Currently, we are using the viewfinder stream to take the picture. + // Should be corrected. + options->viewfinder_width = c.camera_hardware_.resolution_x_; + options->viewfinder_height = c.camera_hardware_.resolution_y_; + options->width = c.camera_hardware_.resolution_x_; + options->height = c.camera_hardware_.resolution_y_; + options->shutter.set( + "11111us"); // Not actually used for external triggering. Just needs to + // be set to something + options->info_text = ""; - try - { - still_image_event_loop(*app, img); - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); - return false; + // We know we are using camera 2 + if (GolfSimCamera::kSystemSlot2CameraOrientation == + CameraHardware::CameraOrientation::kUpsideDown) { + // Tell libcamera to flip the image vertically back to where it should be + options->transform = libcamera::Transform::VFlip; + GS_LOG_MSG(trace, "Flipping still picture upside down."); + } else { + GS_LOG_MSG(trace, "NOT flipping still picture upside down."); } - if (!DeConfigureForLibcameraStill(GolfSimOptions::GetCommandLineOptions().GetCameraNumber())) { - GS_LOG_TRACE_MSG(error, "failed to DeConfigureForLibcameraStill."); - return false; + if (!SetLibcameraTuningFileEnvVariable(c)) { + GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); + return false; } - return true; -} - + if (options->verbose >= 2) + options->Print(); + // This will block until the loop ends + ball_flight_camera_event_loop(app, raw_image); + } catch (std::exception const &e) { + GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + return false; + } -// TBD - This really seems like it should exist in the gs_camera module? -bool TakeRawPicture(const GolfSimCamera& camera, cv::Mat& img) { + // GS_LOG_TRACE_MSG(trace, "Tearing down initial camera."); + app.StopCamera(); // TBD - Need? + app.Teardown(); // TBD - Need? - const GsCameraNumber camera_number = camera.camera_hardware_.camera_number_; + // LoggingTools::LogImage("", raw_image, std::vector < cv::Point >{}, true, + // "InitialRawImageCam2.png"); - const CameraHardware::CameraModel camera_model = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraType : GolfSimCamera::kSystemSlot2CameraType; + // Save the image in memory after un-distorting it for the local camera/lens - // Ensure we have full resolution - ConfigCameraForFullScreenWatching(camera); + return_image = + golf_sim::LibCameraInterface::undistort_camera_image(raw_image, c); - cv::Mat initialImg; - if (!TakeLibcameraStill(camera, initialImg)) { - GS_LOG_MSG(error, "Failed to take still picture."); - return false; - } + if (GolfSimOptions::GetCommandLineOptions().camera_still_mode_) { - if (initialImg.empty()) { - return false; + std::string output_fname = + GolfSimOptions::GetCommandLineOptions().output_filename_; + if (output_fname.empty()) { + output_fname = LoggingTools::kDefaultSaveFileName; } - img = golf_sim::LibCameraInterface::undistort_camera_image(initialImg, camera); + LoggingTools::LogImage("", return_image, std::vector{}, true, + output_fname); + } - return true; + return true; } -// Enhanced ball detection using YOLO when configured -bool CheckForBallEnhanced(GolfBall& ball, cv::Mat& img) { - bool use_yolo = (golf_sim::BallImageProc::kBallPlacementDetectionMethod == "experimental"); - - GsCameraNumber camera_number = GolfSimOptions::GetCommandLineOptions().GetCameraNumber(); - const CameraHardware::CameraModel camera_model = (camera_number == GsCameraNumber::kGsCamera1) ? - GolfSimCamera::kSystemSlot1CameraType : GolfSimCamera::kSystemSlot2CameraType; - const CameraHardware::LensType camera_lens_type = (camera_number == GsCameraNumber::kGsCamera1) ? - GolfSimCamera::kSystemSlot1LensType : GolfSimCamera::kSystemSlot2LensType; - const CameraHardware::CameraOrientation camera_orientation = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraOrientation : GolfSimCamera::kSystemSlot2CameraOrientation; - - GolfSimCamera camera; - camera.camera_hardware_.init_camera_parameters(camera_number, camera_model, camera_lens_type, camera_orientation); - - if (!TakeRawPicture(camera, img)) { - GS_LOG_MSG(error, "Failed to TakeRawPicture."); - return false; - } - - cv::Vec2i search_center = camera.GetExpectedBallCenter(); - - if (use_yolo) { - if (!golf_sim::BallImageProc::PreloadYOLOModel()) { - GS_LOG_MSG(warning, "YOLO model not available, using legacy detection"); - } else { - std::vector detected_circles; - bool detected = golf_sim::BallImageProc::DetectBallsONNX(img, - golf_sim::BallImageProc::BallSearchMode::kFindPlacedBall, - detected_circles); - - if (detected && !detected_circles.empty()) { - GsCircle best_circle; - float best_distance = FLT_MAX; - - for (const auto& circle : detected_circles) { - float dx = circle[0] - search_center[0]; - float dy = circle[1] - search_center[1]; - float distance = sqrt(dx*dx + dy*dy); - - if (distance < best_distance) { - best_distance = distance; - best_circle = circle; - } - } - - if (best_distance < 200) { - ball.ball_circle_ = best_circle; - ball.measured_radius_pixels_ = best_circle[2]; - ball.search_area_center_ = search_center; - ball.search_area_radius_ = 200; - - return true; - } - } - } - } - - bool expectBall = false; - return camera.GetCalibratedBall(camera, img, ball, search_center, expectBall); -} - -// TBD - This really seems like it should exist in the gs_camera module? -bool CheckForBall(GolfBall& ball, cv::Mat& img) { - return CheckForBallEnhanced(ball, img); -} +bool PerformCameraSystemStartup() { -bool CheckForBallLegacy(GolfBall& ball, cv::Mat& img) { + SetLibCameraLoggingOff(); - GsCameraNumber camera_number = GolfSimOptions::GetCommandLineOptions().GetCameraNumber(); + // Setup the Pi Camera to be internally or externally triggered as appropriate - // Figure out where the ball is - // TBD - This repeats the camera initialization that we just did - const CameraHardware::CameraModel camera_model = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraType : GolfSimCamera::kSystemSlot2CameraType; - const CameraHardware::LensType camera_lens_type = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1LensType : GolfSimCamera::kSystemSlot2LensType; - const CameraHardware::CameraOrientation camera_orientation = (camera_number == GsCameraNumber::kGsCamera1) ? GolfSimCamera::kSystemSlot1CameraOrientation : GolfSimCamera::kSystemSlot2CameraOrientation; + SystemMode mode = GolfSimOptions::GetCommandLineOptions().system_mode_; - GolfSimCamera camera; - camera.camera_hardware_.init_camera_parameters(camera_number, camera_model, camera_lens_type, camera_orientation); - camera.camera_hardware_.firstCannedImageFileName = std::string("/mnt/VerdantShare/dev/GolfSim/LM/Images/") + "FirstWaitingImage"; - camera.camera_hardware_.firstCannedImage = img; + switch (mode) { - // If we are checking to see if a ball - if (!TakeRawPicture(camera, img)) { - GS_LOG_MSG(error, "Failed to TakeRawPicture."); - return false; - } + case SystemMode::kCamera1: + case SystemMode::kCamera1TestStandalone: + case SystemMode::kTestSpin: { - cv::Vec2i search_area_center = camera.GetExpectedBallCenter(); + if (!GolfSimOptions::GetCommandLineOptions().run_single_pi_) { + std::string trigger_mode_command = + "sudo " + "$PITRAC_ROOT/ImageProcessing/CameraTools/" + "setCameraTriggerInternal.sh"; - bool expectBall = false; - bool success = camera.GetCalibratedBall(camera, img, ball, search_area_center, expectBall); + int command_result = system(trigger_mode_command.c_str()); - if (!success) { + if (command_result != 0) { + GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); return false; + } + } else { + /**** + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot1CameraType; if (camera_model == + CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { std::string + trigger_mode_command = + "$PITRAC_ROOT/ImageProcessing/CameraTools/imx296_trigger 6 0"; + + GS_LOG_TRACE_MSG(trace, "Camera 1 trigger_mode_command = " + + trigger_mode_command); int command_result = + system(trigger_mode_command.c_str()); + + if (command_result != 0) { + GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); + return false; + } + } + else { + GS_LOG_TRACE_MSG(trace, "Running in single-pi mode, so not setting + camera triggering (internal or external) programmatically. Instead, + please see the following discussion on how to setup the + boot/firmware.config.txt dtoverlays for triggering: + https://forums.raspberrypi.com/viewtopic.php?p=2315464#p2315464."); + } + ****/ } + } break; - return true; -} - - -// The following code is only relevant to the camera 2 system -bool WaitForCam2Trigger(cv::Mat& return_image) { - LibcameraJpegApp app; - - cv::Mat raw_image; - - // Create a camera just to set the resolution and for un-distort operation - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot2CameraType; - const CameraHardware::LensType camera_lens_type = GolfSimCamera::kSystemSlot2LensType; - const CameraHardware::CameraOrientation camera_orientation = GolfSimCamera::kSystemSlot2CameraOrientation; + case SystemMode::kCamera2: + case SystemMode::kRunCam2ProcessForPi1Processing: + case SystemMode::kCamera2TestStandalone: { + if (!GolfSimOptions::GetCommandLineOptions().run_single_pi_) { + std::string trigger_mode_command = + "sudo " + "$PITRAC_ROOT/ImageProcessing/CameraTools/" + "setCameraTriggerExternal.sh"; - GolfSimCamera c; - c.camera_hardware_.init_camera_parameters(GsCameraNumber::kGsCamera2, camera_model, camera_lens_type, camera_orientation); - - try - { - StillOptions* options = app.GetOptions(); - - char dummy_arguments[] = "DummyExecutableName"; - char* argv[] = { dummy_arguments, NULL }; - - if (!options->Parse(1, argv)) - { - return -1; - } - - SetLibCameraLoggingOff(); + int command_result = system(trigger_mode_command.c_str()); - // On a two-Pi system, each Pi has just one camera, and that camera will be in slot 0 - // On a single-pi system the one Pi 5 has both cameras. And Camera 2 will be in slot 1 - // because Camera 1 is in slot 0. - if (GolfSimOptions::GetCommandLineOptions().run_single_pi_) { - options->camera = 1; - } - else { - options->camera = 0; - } - - if (GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2Calibrate || - GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2BallLocation || - GolfSimOptions::GetCommandLineOptions().system_mode_ == SystemMode::kCamera2AutoCalibrate) { - - options->gain = LibCameraInterface::kCamera2CalibrateOrLocationGain; - } - else if (GolfSimClubs::GetCurrentClubType() == GolfSimClubs::kPutter) { - options->gain = LibCameraInterface::kCamera2PuttingGain; - options->contrast = LibCameraInterface::kCamera2PuttingContrast; - } - else { - if (!GolfSimOptions::GetCommandLineOptions().lm_comparison_mode_) { - options->gain = LibCameraInterface::kCamera2Gain; - } - else { - options->gain = LibCameraInterface::kCamera2ComparisonGain; - } - - options->contrast = LibCameraInterface::kCamera2Contrast; - } - - options->saturation = LibCameraInterface::kCamera2Saturation; - GS_LOG_MSG(trace, "Flight camera saturation = " + std::to_string(options->saturation)); - - options->immediate = true; - options->timeout.set("0ms"); // Wait forever for external trigger - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot1CameraType; - if (camera_model != CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { - options->denoise = "cdn_off"; - } - else { - options->denoise = "auto"; - } - - GS_LOG_TRACE_MSG(trace, "Camera denoise option set to: " + options->denoise); - options->nopreview = true; - // TBD - Currently, we are using the viewfinder stream to take the picture. Should be corrected. - options->viewfinder_width = c.camera_hardware_.resolution_x_; - options->viewfinder_height = c.camera_hardware_.resolution_y_; - options->width = c.camera_hardware_.resolution_x_; - options->height = c.camera_hardware_.resolution_y_; - options->shutter.set("11111us"); // Not actually used for external triggering. Just needs to be set to something - options->info_text = ""; - - // We know we are using camera 2 - if (GolfSimCamera::kSystemSlot2CameraOrientation == CameraHardware::CameraOrientation::kUpsideDown) { - // Tell libcamera to flip the image vertically back to where it should be - options->transform = libcamera::Transform::VFlip; - GS_LOG_MSG(trace, "Flipping still picture upside down."); - } - else { - GS_LOG_MSG(trace, "NOT flipping still picture upside down."); - } - - if (!SetLibcameraTuningFileEnvVariable(c)) { - GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); - return false; - } - - if (options->verbose >= 2) - options->Print(); - - // This will block until the loop ends - ball_flight_camera_event_loop(app, raw_image); - } - catch (std::exception const& e) - { - GS_LOG_MSG(error, "ERROR: *** " + std::string(e.what()) + " ***"); + if (command_result != 0) { + GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); return false; - } - - // GS_LOG_TRACE_MSG(trace, "Tearing down initial camera."); - app.StopCamera(); // TBD - Need? - app.Teardown(); // TBD - Need? + } + } else { + const CameraHardware::CameraModel camera_model = + GolfSimCamera::kSystemSlot2CameraType; - // LoggingTools::LogImage("", raw_image, std::vector < cv::Point >{}, true, "InitialRawImageCam2.png"); + if (camera_model == CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { + std::string trigger_mode_command = + "$PITRAC_ROOT/ImageProcessing/CameraTools/imx296_trigger 4 1"; - // Save the image in memory after un-distorting it for the local camera/lens + int command_result = system(trigger_mode_command.c_str()); - return_image = golf_sim::LibCameraInterface::undistort_camera_image(raw_image, c); - - if (GolfSimOptions::GetCommandLineOptions().camera_still_mode_ ) { - - std::string output_fname = GolfSimOptions::GetCommandLineOptions().output_filename_; - if (output_fname.empty()) { - output_fname = LoggingTools::kDefaultSaveFileName; + if (command_result != 0) { + GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); + return false; } - - LoggingTools::LogImage("", return_image, std::vector < cv::Point >{}, true, output_fname); + } } - return true; -} - - -bool PerformCameraSystemStartup() { - - SetLibCameraLoggingOff(); - - // Setup the Pi Camera to be internally or externally triggered as appropriate - - SystemMode mode = GolfSimOptions::GetCommandLineOptions().system_mode_; - - switch (mode) { - - case SystemMode::kCamera1: - case SystemMode::kCamera1TestStandalone: - case SystemMode::kTestSpin: { - - if (!GolfSimOptions::GetCommandLineOptions().run_single_pi_) { - std::string trigger_mode_command = "sudo $PITRAC_ROOT/ImageProcessing/CameraTools/setCameraTriggerInternal.sh"; - - int command_result = system(trigger_mode_command.c_str()); - - if (command_result != 0) { - GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); - return false; - } - } - else { - /**** - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot1CameraType; - if (camera_model == CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { - std::string trigger_mode_command = "$PITRAC_ROOT/ImageProcessing/CameraTools/imx296_trigger 6 0"; - - GS_LOG_TRACE_MSG(trace, "Camera 1 trigger_mode_command = " + trigger_mode_command); - int command_result = system(trigger_mode_command.c_str()); - - if (command_result != 0) { - GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); - return false; - } - } - else { - GS_LOG_TRACE_MSG(trace, "Running in single-pi mode, so not setting camera triggering (internal or external) programmatically. Instead, please see the following discussion on how to setup the boot/firmware.config.txt dtoverlays for triggering: https://forums.raspberrypi.com/viewtopic.php?p=2315464#p2315464."); - } - ****/ - } - } - break; - - case SystemMode::kCamera2: - case SystemMode::kRunCam2ProcessForPi1Processing: - case SystemMode::kCamera2TestStandalone: { - - if (!GolfSimOptions::GetCommandLineOptions().run_single_pi_) { - std::string trigger_mode_command = "sudo $PITRAC_ROOT/ImageProcessing/CameraTools/setCameraTriggerExternal.sh"; - - int command_result = system(trigger_mode_command.c_str()); - - if (command_result != 0) { - GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); - return false; - } - } - else { - const CameraHardware::CameraModel camera_model = GolfSimCamera::kSystemSlot2CameraType; - - if (camera_model == CameraHardware::CameraModel::InnoMakerIMX296GS_Mono) { - std::string trigger_mode_command = "$PITRAC_ROOT/ImageProcessing/CameraTools/imx296_trigger 4 1"; - - int command_result = system(trigger_mode_command.c_str()); - - if (command_result != 0) { - GS_LOG_TRACE_MSG(trace, "system(trigger_mode_command) failed."); - return false; - } - } - } - - // Create a camera just for purposes of setting the tuning file variable - GolfSimCamera camera; - camera.camera_hardware_.init_camera_parameters(GsCameraNumber::kGsCamera2, GolfSimCamera::kSystemSlot2CameraType, GolfSimCamera::kSystemSlot2LensType, GolfSimCamera::kSystemSlot1CameraOrientation); - - if (!SetLibcameraTuningFileEnvVariable(camera)) { - GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); - return false; - } - } - break; - - case SystemMode::kTest: - default: - break; + // Create a camera just for purposes of setting the tuning file variable + GolfSimCamera camera; + camera.camera_hardware_.init_camera_parameters( + GsCameraNumber::kGsCamera2, GolfSimCamera::kSystemSlot2CameraType, + GolfSimCamera::kSystemSlot2LensType, + GolfSimCamera::kSystemSlot1CameraOrientation); + + if (!SetLibcameraTuningFileEnvVariable(camera)) { + GS_LOG_TRACE_MSG(error, "failed to SetLibcameraTuningFileEnvVariable"); + return false; } + } break; - return true; -} + case SystemMode::kTest: + default: + break; + } + return true; } +} // namespace golf_sim + #endif // #ifdef __unix__ // Ignore in Windows environment // Unix-only libcamera-related code diff --git a/Software/LMSourceCode/ImageProcessing/libcamera_interface.h b/Software/LMSourceCode/ImageProcessing/libcamera_interface.h index 729fce55..ec17672f 100644 --- a/Software/LMSourceCode/ImageProcessing/libcamera_interface.h +++ b/Software/LMSourceCode/ImageProcessing/libcamera_interface.h @@ -3,11 +3,12 @@ * Copyright (C) 2022-2025, Verdant Consultants, LLC. */ -// This is the main interface between the LM and the underlying libcamera system. +// This is the main interface between the LM and the underlying libcamera +// system. #pragma once -#ifdef __unix__ // Ignore in Windows environment +#ifdef __unix__ // Ignore in Windows environment #include "core/rpicam_app.hpp" #include "core/rpicam_encoder.hpp" @@ -21,124 +22,130 @@ #include "still_image_libcamera_app.hpp" - - namespace golf_sim { - // TBD - Put in a struct or class sometime - - class LibCameraInterface { - public: - - // The Camera 1 operates in a cropped mode only when watching the teed-up ball for a - // hit. Otherwise, while watching for the ball to be teed up in the first place, the - // camera operates at full resolution. - enum CropConfiguration { - kCropUnknown, - kFullScreen, - kCropped - }; - - enum CameraConfiguration { - kNotConfigured, - kStillPicture, - kHighSpeedWatching, - kExternallyStrobed - }; - - static cv::Mat undistort_camera_image(const cv::Mat& img, const GolfSimCamera& camera); - static bool SendCamera2PreImage(const cv::Mat& raw_image); - - static uint kMaxWatchingCropWidth; - static uint kMaxWatchingCropHeight; - static double kCamera1Gain; // 0.0 to TBD?? - static double kCamera1Saturation; - static double kCamera1HighFPSGain; // 15.0 to TBD?? - static double kCamera1Contrast; // 0.0 to 32.0 - static double kCamera2Gain; // 0.0 to TBD?? - static double kCamera2Saturation; - static double kCamera2ComparisonGain; // 0.0 to TBD?? - static double kCamera2CalibrateOrLocationGain; - static double kCamera2StrobedEnvironmentGain; - static double kCamera2Contrast; // 0.0 to 32.0 - static double kCamera2PuttingGain; // 0.0 to TBD?? - static double kCamera2PuttingContrast; // 0.0 to 32.0 - static std::string kCameraMotionDetectSettings; - - static long kCamera1StillShutterTimeuS; - static long kCamera2StillShutterTimeuS; - - // Once the cropped rectange is determined (usually around the center of the ball) - // These offsets can further move that cropping area - static int kCroppedImagePixelOffsetLeft; - static int kCroppedImagePixelOffsetUp; - - - static CropConfiguration camera_crop_configuration_; - static cv::Vec2i current_watch_resolution_; - static cv::Vec2i current_watch_offset_; - - - // The first (0th) element in the array is for camera1, the second for camera2 - static CameraConfiguration libcamera_configuration_[]; - static LibcameraJpegApp* libcamera_app_[]; - - // True (or set to a non-negative number) if we've already figured out the media and device number for the camera; - static bool camera_location_found_; - static int previously_found_media_number_; - static int previously_found_device_number_; - }; - - bool TakeRawPicture(const GolfSimCamera& camera, cv::Mat& img); - - // Takes a picture and then tries to find the ball - bool CheckForBall(GolfBall& ball, cv::Mat& return_image); - - // Configures the camera and the rest of the system to sit in a tight loop, waiting for the - // ball to move. Blocks until movement or some other event that causes the loop to stop - // Returns whether or not motion was detected - // Lower-level methods in the loop will try to trigger the external shutter of the camera 2 - // as soon as possible after motion has been detected. - bool WatchForBallMovement(GolfSimCamera& camera, const GolfBall& ball, bool & motion_detected); - - // Do everything necessary to get the system ready to use a tightly-cropped camera video - // mode (in order to allow high FPS) - bool ConfigCameraForCropping(GolfBall ball, GolfSimCamera& camera, RPiCamEncoder& app); - - // Uses media-ctl to setup a cropping mode to allow for high FPS. Requires GS camera. - bool SendCameraCroppingCommand(const GolfSimCamera &camera, cv::Vec2i& cropping_window_size, cv::Vec2i& cropping_window_offset); - - // Sets up the rpicam-app-based post-processing pipeline so that the motion-detection stage knows - // how to analyze the cropped image - bool ConfigurePostProcessing(const cv::Vec2i& roi_size, const cv::Vec2i& roi_offset); - - // Sets up a libcamera encoder with options necessary for a high FPS video loop in a cropped part of - // the camera sensor. - bool ConfigureLibCameraOptions(const GolfSimCamera& camera, RPiCamEncoder& app, const cv::Vec2i& cropping_window_size, uint cropped_frame_rate_fps_fps); - - std::string GetCmdLineForMediaCtlCropping(const GolfSimCamera &camera_number, cv::Vec2i croppedHW, cv::Vec2i cropOffsetXY); - - // Determine where the camera is, such as /dev/media2 at device 6 - bool DiscoverCameraLocation(const GsCameraNumber camera_number, int& media_number, int& device_number); - - bool RetrieveCameraInfo(const GsCameraNumber camera_number, cv::Vec2i& resolution, uint& frameRate, bool restartCamera = false); - - LibcameraJpegApp* ConfigureForLibcameraStill(const GolfSimCamera& camera); - bool DeConfigureForLibcameraStill(const GsCameraNumber camera_number); - - bool TakeLibcameraStill(const GolfSimCamera& camera, cv::Mat& return_image); - - bool WatchForHitAndTrigger(const GolfBall& ball, cv::Mat& return_image, bool& motion_detected); - - // TBD - REMOVE bool ConfigCameraForCropping(const GolfSimCamera& c); - - bool WaitForCam2Trigger(cv::Mat& return_image); - - bool PerformCameraSystemStartup(); - - static void SetLibCameraLoggingOff(); - - -} +// TBD - Put in a struct or class sometime + +class LibCameraInterface { +public: + // The Camera 1 operates in a cropped mode only when watching the teed-up ball + // for a hit. Otherwise, while watching for the ball to be teed up in the + // first place, the camera operates at full resolution. + enum CropConfiguration { kCropUnknown, kFullScreen, kCropped }; + + enum CameraConfiguration { + kNotConfigured, + kStillPicture, + kHighSpeedWatching, + kExternallyStrobed + }; + + static cv::Mat undistort_camera_image(const cv::Mat &img, + const GolfSimCamera &camera); + static bool SendCamera2PreImage(const cv::Mat &raw_image); + + static uint kMaxWatchingCropWidth; + static uint kMaxWatchingCropHeight; + static double kCamera1Gain; // 0.0 to TBD?? + static double kCamera1Saturation; + static double kCamera1HighFPSGain; // 15.0 to TBD?? + static double kCamera1Contrast; // 0.0 to 32.0 + static double kCamera2Gain; // 0.0 to TBD?? + static double kCamera2Saturation; // 0.0 = greyscale, 1.0 = normal + static double kCamera2ComparisonGain; // 0.0 to TBD?? + static double kCamera2CalibrateOrLocationGain; + static double kCamera2StrobedEnvironmentGain; + static double kCamera2Contrast; // 0.0 to 32.0 + static double kCamera2PuttingGain; // 0.0 to TBD?? + static double kCamera2PuttingContrast; // 0.0 to 32.0 + static std::string kCameraMotionDetectSettings; + + static long kCamera1StillShutterTimeuS; + static long kCamera2StillShutterTimeuS; + + // Once the cropped rectange is determined (usually around the center of the + // ball) These offsets can further move that cropping area + static int kCroppedImagePixelOffsetLeft; + static int kCroppedImagePixelOffsetUp; + + static CropConfiguration camera_crop_configuration_; + static cv::Vec2i current_watch_resolution_; + static cv::Vec2i current_watch_offset_; + + // The first (0th) element in the array is for camera1, the second for camera2 + static CameraConfiguration libcamera_configuration_[]; + static LibcameraJpegApp *libcamera_app_[]; + + // True (or set to a non-negative number) if we've already figured out the + // media and device number for the camera; + static bool camera_location_found_; + static int previously_found_media_number_; + static int previously_found_device_number_; +}; + +bool TakeRawPicture(const GolfSimCamera &camera, cv::Mat &img); + +// Takes a picture and then tries to find the ball +bool CheckForBall(GolfBall &ball, cv::Mat &return_image); + +// Configures the camera and the rest of the system to sit in a tight loop, +// waiting for the ball to move. Blocks until movement or some other event that +// causes the loop to stop Returns whether or not motion was detected +// Lower-level methods in the loop will try to trigger the external shutter of +// the camera 2 as soon as possible after motion has been detected. +bool WatchForBallMovement(GolfSimCamera &camera, const GolfBall &ball, + bool &motion_detected); + +// Do everything necessary to get the system ready to use a tightly-cropped +// camera video mode (in order to allow high FPS) +bool ConfigCameraForCropping(GolfBall ball, GolfSimCamera &camera, + RPiCamEncoder &app); + +// Uses media-ctl to setup a cropping mode to allow for high FPS. Requires GS +// camera. +bool SendCameraCroppingCommand(const GolfSimCamera &camera, + cv::Vec2i &cropping_window_size, + cv::Vec2i &cropping_window_offset); + +// Sets up the rpicam-app-based post-processing pipeline so that the +// motion-detection stage knows how to analyze the cropped image +bool ConfigurePostProcessing(const cv::Vec2i &roi_size, + const cv::Vec2i &roi_offset); + +// Sets up a libcamera encoder with options necessary for a high FPS video loop +// in a cropped part of the camera sensor. +bool ConfigureLibCameraOptions(const GolfSimCamera &camera, RPiCamEncoder &app, + const cv::Vec2i &cropping_window_size, + uint cropped_frame_rate_fps_fps); + +std::string GetCmdLineForMediaCtlCropping(const GolfSimCamera &camera_number, + cv::Vec2i croppedHW, + cv::Vec2i cropOffsetXY); + +// Determine where the camera is, such as /dev/media2 at device 6 +bool DiscoverCameraLocation(const GsCameraNumber camera_number, + int &media_number, int &device_number); + +bool RetrieveCameraInfo(const GsCameraNumber camera_number, + cv::Vec2i &resolution, uint &frameRate, + bool restartCamera = false); + +LibcameraJpegApp *ConfigureForLibcameraStill(const GolfSimCamera &camera); +bool DeConfigureForLibcameraStill(const GsCameraNumber camera_number); + +bool TakeLibcameraStill(const GolfSimCamera &camera, cv::Mat &return_image); + +bool WatchForHitAndTrigger(const GolfBall &ball, cv::Mat &return_image, + bool &motion_detected); + +// TBD - REMOVE bool ConfigCameraForCropping(const GolfSimCamera& c); + +bool WaitForCam2Trigger(cv::Mat &return_image); + +bool PerformCameraSystemStartup(); + +static void SetLibCameraLoggingOff(); + +} // namespace golf_sim #endif // #ifdef __unix__ // Ignore in Windows environment diff --git a/Software/LMSourceCode/ImageProcessing/meson.build b/Software/LMSourceCode/ImageProcessing/meson.build index c4bfe389..40c8339e 100644 --- a/Software/LMSourceCode/ImageProcessing/meson.build +++ b/Software/LMSourceCode/ImageProcessing/meson.build @@ -216,6 +216,8 @@ pitrac_lm_sources += ([ 'gs_ipc_system.cpp', 'gs_message_consumer.cpp', 'gs_message_producer.cpp', + 'gs_opengolfsim_interface.cpp', + 'gs_opengolfsim_results.cpp' ]) pitrac_lm_sources += ([ diff --git a/Software/web-server/configurations.json b/Software/web-server/configurations.json index 4c1766e7..e37d02d4 100644 --- a/Software/web-server/configurations.json +++ b/Software/web-server/configurations.json @@ -64,19 +64,14 @@ "displayName": "Model Search Paths", "description": "Directories to search for AI models", "type": "array", - "default": [ - "/etc/pitrac/models" - ], + "default": ["/etc/pitrac/models"], "internal": true }, "modelFilePatterns": { "displayName": "Model File Patterns", "description": "File patterns to match when searching for models", "type": "array", - "default": [ - "best.onnx", - "weights/best.onnx" - ], + "default": ["best.onnx", "weights/best.onnx"], "internal": true } }, @@ -675,6 +670,24 @@ "passedVia": "json", "passedTo": "both" }, + "gs_config.cameras.kCamera2Saturation": { + "category": "Cameras", + "subcategory": "basic", + "basicSubcategory": "Cameras", + "displayName": "Camera 2 Saturation", + "description": "Saturation setting for Camera 2 - 0.0 for black and white images", + "visibleWhen": { + "system.mode": "single" + }, + "type": "number", + "min": 0.0, + "max": 2.0, + "step": 0.1, + "default": 0.0, + "requiresRestart": true, + "passedVia": "json", + "passedTo": "both" + }, "gs_config.cameras.kCamera1SearchCenterX": { "category": "Cameras", "subcategory": "advanced", @@ -789,6 +802,32 @@ "passedVia": "json", "passedTo": "both" }, + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectAddress": { + "category": "Simulators", + "subcategory": "basic", + "basicSubcategory": "Simulators", + "displayName": "Open Golf Sim Address", + "description": "IP address of Open Golf Sim simulator (e.g., 192.168.1.100)", + "type": "ip_address", + "default": "", + "requiresRestart": false, + "passedVia": "json", + "passedTo": "both" + }, + "gs_config.golf_simulator_interfaces.OpenGolfSim.kOGSConnectPort": { + "category": "Simulators", + "subcategory": "basic", + "basicSubcategory": "Simulators", + "displayName": "Open Golf Sim Port", + "description": "Port number for Open Golf simulator", + "type": "number", + "min": 1, + "max": 65535, + "default": 3111, + "requiresRestart": true, + "passedVia": "json", + "passedTo": "both" + }, "gs_config.ball_identification.kUseCLAHEProcessing": { "category": "Ball Detection", "subcategory": "basic", @@ -1417,7 +1456,7 @@ "type": "number", "min": 0.02, "max": 0.025, - "step": 1e-06, + "step": 1e-6, "default": 0.021335, "requiresRestart": false, "internal": true, @@ -2478,16 +2517,7 @@ "displayName": "Strobe Pulse Vector Driver", "description": "Strobe timing vector for driver shots", "type": "array", - "default": [ - 0.7, - 1.8, - 3.0, - 2.2, - 3.0, - 7.1, - 4.0, - 0 - ], + "default": [0.7, 1.8, 3.0, 2.2, 3.0, 7.1, 4.0, 0], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -2498,14 +2528,7 @@ "displayName": "Strobe Pulse Vector Putter", "description": "Strobe timing vector for putting", "type": "array", - "default": [ - 5.0, - 30.0, - 30.0, - 30.0, - 50.0, - 0 - ], + "default": [5.0, 30.0, 30.0, 30.0, 50.0, 0], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -3660,19 +3683,7 @@ "description": "Extended strobe timing vector for driver shots", "type": "array", "default": [ - 0.175, - 0.7, - 1.4, - 2.45, - 1.26, - 2.8, - 2.1, - 3.15, - 3.85, - 3.85, - 10.4, - 3.5, - 0 + 0.175, 0.7, 1.4, 2.45, 1.26, 2.8, 2.1, 3.15, 3.85, 3.85, 10.4, 3.5, 0 ], "requiresRestart": true, "passedVia": "json", @@ -3684,9 +3695,7 @@ "displayName": "Dynamic Follow-On Pulse Vector Putter", "description": "Dynamic follow-on pulse timing for putting", "type": "array", - "default": [ - 444.0 - ], + "default": [444.0], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -3852,21 +3861,9 @@ "gs_config.cameras.kCamera1CalibrationMatrix": { "type": "array", "default": [ - [ - "1833.5291988027575", - "0.0", - "697.2791579239232" - ], - [ - "0.0", - "1832.2499845181273", - "513.0904087097207" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1833.5291988027575", "0.0", "697.2791579239232"], + ["0.0", "1832.2499845181273", "513.0904087097207"], + ["0.0", "0.0", "1.0"] ], "description": "Camera 1 intrinsic calibration matrix", "category": "calibration", @@ -3888,21 +3885,9 @@ "gs_config.cameras.kCamera2CalibrationMatrix": { "type": "array", "default": [ - [ - "2340.2520648903665", - "0.0", - "698.4611375636877" - ], - [ - "0.0", - "2318.2676880118993", - "462.7245851119162" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["2340.2520648903665", "0.0", "698.4611375636877"], + ["0.0", "2318.2676880118993", "462.7245851119162"], + ["0.0", "0.0", "1.0"] ], "description": "Camera 2 intrinsic calibration matrix", "category": "calibration", @@ -3924,21 +3909,9 @@ "gs_config.cameras.3_6Lens_kCamera1CalibrationMatrix": { "type": "array", "default": [ - [ - "1748.506644661262953", - "0.0", - "632.5374407393054526" - ], - [ - "0.0", - "1743.341748687922745", - "407.5677927449370941" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1748.506644661262953", "0.0", "632.5374407393054526"], + ["0.0", "1743.341748687922745", "407.5677927449370941"], + ["0.0", "0.0", "1.0"] ], "description": "Camera 1 calibration matrix for 3.6mm lens", "category": "calibration", @@ -3960,21 +3933,9 @@ "gs_config.cameras.3_6Lens_kCamera2CalibrationMatrix": { "type": "array", "default": [ - [ - "1065.366451276604266", - "0.0", - "715.3187116996945178" - ], - [ - "0.0", - "1068.642040947899886", - "509.4612905764302582" - ], - [ - "0.0", - "0.0", - "1.0" - ] + ["1065.366451276604266", "0.0", "715.3187116996945178"], + ["0.0", "1068.642040947899886", "509.4612905764302582"], + ["0.0", "0.0", "1.0"] ], "description": "Camera 2 calibration matrix for 3.6mm lens", "category": "calibration", @@ -3995,53 +3956,35 @@ }, "gs_config.cameras.kCamera1Angles": { "type": "array", - "default": [ - "2.1406460383579446", - "-26.426049413957287" - ], + "default": ["2.1406460383579446", "-26.426049413957287"], "description": "Camera 1 rotation angles (pitch, yaw)", "category": "calibration", "ui_name": "Camera 1 Angles" }, "gs_config.cameras.kCamera1PositionsFromExpectedBallMeters": { "type": "array", - "default": [ - "-0.200", - "-0.234", - "0.54" - ], + "default": ["-0.200", "-0.234", "0.54"], "description": "Camera 1 position relative to expected ball location (meters)", "category": "calibration", "ui_name": "Camera 1 Position" }, "gs_config.cameras.kCamera2Angles": { "type": "array", - "default": [ - "-3.9096762712853037", - "10.292573196795921" - ], + "default": ["-3.9096762712853037", "10.292573196795921"], "description": "Camera 2 rotation angles (pitch, yaw)", "category": "calibration", "ui_name": "Camera 2 Angles" }, "gs_config.cameras.kCamera2OffsetFromCamera1OriginMeters": { "type": "array", - "default": [ - "0.00", - "-0.19", - "0.0" - ], + "default": ["0.00", "-0.19", "0.0"], "description": "Camera 2 offset from Camera 1 origin (meters)", "category": "calibration", "ui_name": "Camera 2 Offset" }, "gs_config.cameras.kCamera2PositionsFromExpectedBallMeters": { "type": "array", - "default": [ - "0.0", - "-0.051", - "0.45" - ], + "default": ["0.0", "-0.051", "0.45"], "description": "Camera 2 position relative to expected ball location (meters)", "category": "calibration", "ui_name": "Camera 2 Position" @@ -4068,11 +4011,7 @@ "displayName": "Custom Calibration Rig Ball Position (x,y,z) (in meters) for camera 1", "description": "Position of ball if using non-standard calibration rig", "type": "array", - "default": [ - -0.120, - -0.28, - 0.44 - ], + "default": [-0.12, -0.28, 0.44], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4083,11 +4022,7 @@ "displayName": "Custom Calibration Rig Ball Position (x,y,z) (in meters) for camera 2", "description": "Position of ball if using non-standard calibration rig", "type": "array", - "default": [ - "0.00", - "0.095", - "0.435" - ], + "default": ["0.00", "0.095", "0.435"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4098,11 +4033,7 @@ "displayName": "Auto Cal. Ball Position Cam1 - Straight-Out Positioning Using V2 Enclosure", "description": "Baseline calibration rig ball position from Camera 1 (meters) for both cameras pointing straight out the centerline", "type": "array", - "default": [ - "-0.12", - "-0.28", - "0.44" - ], + "default": ["-0.12", "-0.28", "0.44"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4113,11 +4044,7 @@ "displayName": "Auto Cal. Ball Position Cam2 - Straight-Out Positioning Using V2 Enclosure", "description": "Baseline calibration rig ball position from Camera 2 (meters) for both cameras pointing straight out the centerline", "type": "array", - "default": [ - "0.00", - "0.095", - "0.435" - ], + "default": ["0.00", "0.095", "0.435"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4128,11 +4055,7 @@ "displayName": "Auto Cal. Ball Position Cam1 - Skewed Positioning Using V2 Enclosure", "description": "Baseline calibration rig ball position from Camera 1 (meters) for camera 1 skewed back from the centerline", "type": "array", - "default": [ - "-0.52", - "-0.28", - "0.44" - ], + "default": ["-0.52", "-0.28", "0.44"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4143,11 +4066,7 @@ "displayName": "Auto Cal. Ball Position Cam2 - Skewed Positioning Using V2 Enclosure", "description": "Baseline calibration rig ball position from Camera 2 (meters) for camera 1 skewed back from the centerline. Generally, camera 2 is always straight out - only camera 1 is skewed.", "type": "array", - "default": [ - "0.00", - "0.095", - "0.435" - ], + "default": ["0.00", "0.095", "0.435"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4158,11 +4077,7 @@ "displayName": "Auto Cal. Ball Position Cam1 - Straight-Out Positioning Using V3 Enclosure", "description": "Baseline calibration rig ball position from Camera 1 (meters) for both cameras pointing straight out the centerline", "type": "array", - "default": [ - "-0.12", - "-0.243", - "0.481" - ], + "default": ["-0.12", "-0.243", "0.481"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4173,11 +4088,7 @@ "displayName": "Auto Cal. Ball Position Cam2 - Straight-Out Positioning Using V3 Enclosure", "description": "Baseline calibration rig ball position from Camera 2 (meters) for both cameras pointing straight out the centerline", "type": "array", - "default": [ - "0.00", - "0.109", - "0.481" - ], + "default": ["0.00", "0.109", "0.481"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4188,11 +4099,7 @@ "displayName": "Auto Cal. Ball Position Cam1 - Skewed Positioning Using V3 Enclosure", "description": "Baseline calibration rig ball position from Camera 1 (meters) for camera 1 skewed back from the centerline", "type": "array", - "default": [ - "-0.521", - "-0.243", - "0.481" - ], + "default": ["-0.521", "-0.243", "0.481"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4203,11 +4110,7 @@ "displayName": "Auto Cal. Ball Position Cam2 - Skewed Positioning Using V3 Enclosure", "description": "Baseline calibration rig ball position from Camera 2 (meters) for camera 1 skewed back from the centerline. Generally, camera 2 is always straight out - only camera 1 is skewed.", "type": "array", - "default": [ - "0.00", - "0.109", - "0.472" - ], + "default": ["0.00", "0.109", "0.472"], "requiresRestart": true, "passedVia": "json", "passedTo": "both" @@ -4227,121 +4130,121 @@ "ui_name": "ALT Pre-Hough Blur Size" } }, - "categoryList": [ - "System", - "Cameras", - "Simulators", - "Ball Detection", - "AI Detection", - "Storage", - "Network", - "Logging", - "Strobing", - "Spin Analysis", - "Calibration", - "Testing", - "Debugging", - "Club Data", - "Display" - ], - "categories": { - "System": { - "displayName": "System Settings", - "description": "Core system configuration", - "icon": "settings" - }, - "Ball Detection": { - "displayName": "Ball Detection", - "description": "Ball detection algorithms and parameters", - "icon": "target" - }, - "AI Detection": { - "displayName": "AI Detection", - "description": "Neural network detection settings", - "icon": "brain" - }, - "Cameras": { - "displayName": "Cameras", - "description": "Camera hardware and settings", - "icon": "camera" - }, - "Simulators": { - "displayName": "Simulator Interfaces", - "description": "Golf simulator connections", - "icon": "game" - }, - "Strobing": { - "displayName": "LED Strobing", - "description": "LED timing and speed adjustments", - "icon": "flash" - }, - "Storage": { - "displayName": "Storage", - "description": "File paths and directories", - "icon": "folder" - }, - "Logging": { - "displayName": "Logging", - "description": "Image and data logging options", - "icon": "file" - }, - "Network": { - "displayName": "Network", - "description": "Network and communication settings", - "icon": "network" - }, - "Spin Analysis": { - "displayName": "Spin Analysis", - "description": "Spin calculation settings", - "icon": "rotate" - }, - "Calibration": { - "displayName": "Calibration", - "description": "Camera calibration parameters", - "icon": "adjust" - }, - "Advanced": { - "displayName": "Advanced", - "description": "Advanced configuration options", - "icon": "expert" - } - }, - "basicSubcategories": { - "System": { - "displayName": "System", - "order": 1 - }, - "Detection Methods": { - "displayName": "Detection Methods", - "order": 2 - }, - "Cameras": { - "displayName": "Cameras", - "order": 3 - }, - "Simulators": { - "displayName": "Simulators", - "order": 4 - }, - "Ball Detection": { - "displayName": "Ball Detection", - "order": 5 - }, - "Strobing": { - "displayName": "Strobing", - "order": 6 - }, - "Storage": { - "displayName": "Storage", - "order": 7 - }, - "Network": { - "displayName": "Network", - "order": 8 - }, - "Spin": { - "displayName": "Spin", - "order": 9 - } + "categoryList": [ + "System", + "Cameras", + "Simulators", + "Ball Detection", + "AI Detection", + "Storage", + "Network", + "Logging", + "Strobing", + "Spin Analysis", + "Calibration", + "Testing", + "Debugging", + "Club Data", + "Display" + ], + "categories": { + "System": { + "displayName": "System Settings", + "description": "Core system configuration", + "icon": "settings" + }, + "Ball Detection": { + "displayName": "Ball Detection", + "description": "Ball detection algorithms and parameters", + "icon": "target" + }, + "AI Detection": { + "displayName": "AI Detection", + "description": "Neural network detection settings", + "icon": "brain" + }, + "Cameras": { + "displayName": "Cameras", + "description": "Camera hardware and settings", + "icon": "camera" + }, + "Simulators": { + "displayName": "Simulator Interfaces", + "description": "Golf simulator connections", + "icon": "game" + }, + "Strobing": { + "displayName": "LED Strobing", + "description": "LED timing and speed adjustments", + "icon": "flash" + }, + "Storage": { + "displayName": "Storage", + "description": "File paths and directories", + "icon": "folder" + }, + "Logging": { + "displayName": "Logging", + "description": "Image and data logging options", + "icon": "file" + }, + "Network": { + "displayName": "Network", + "description": "Network and communication settings", + "icon": "network" + }, + "Spin Analysis": { + "displayName": "Spin Analysis", + "description": "Spin calculation settings", + "icon": "rotate" + }, + "Calibration": { + "displayName": "Calibration", + "description": "Camera calibration parameters", + "icon": "adjust" + }, + "Advanced": { + "displayName": "Advanced", + "description": "Advanced configuration options", + "icon": "expert" + } + }, + "basicSubcategories": { + "System": { + "displayName": "System", + "order": 1 + }, + "Detection Methods": { + "displayName": "Detection Methods", + "order": 2 + }, + "Cameras": { + "displayName": "Cameras", + "order": 3 + }, + "Simulators": { + "displayName": "Simulators", + "order": 4 + }, + "Ball Detection": { + "displayName": "Ball Detection", + "order": 5 + }, + "Strobing": { + "displayName": "Strobing", + "order": 6 + }, + "Storage": { + "displayName": "Storage", + "order": 7 + }, + "Network": { + "displayName": "Network", + "order": 8 + }, + "Spin": { + "displayName": "Spin", + "order": 9 } } +} diff --git a/Software/web-server/static/css/dashboard.css b/Software/web-server/static/css/dashboard.css index c059549a..dc2bb712 100644 --- a/Software/web-server/static/css/dashboard.css +++ b/Software/web-server/static/css/dashboard.css @@ -2,293 +2,349 @@ /* Metrics Grid */ .metrics-grid { - display: grid; - grid-template-columns: repeat(auto-fit, minmax(280px, 1fr)); - gap: 1.25rem; - margin-bottom: 2rem; + display: grid; + grid-template-columns: repeat(auto-fit, minmax(280px, 1fr)); + gap: 1.25rem; + margin-bottom: 2rem; } /* Metric Cards */ .metric-card { - background: var(--bg-card); - border-radius: 1rem; - overflow: hidden; - box-shadow: var(--shadow-md); - transition: all 0.3s cubic-bezier(0.4, 0, 0.2, 1); - border: 1px solid var(--border-color); + background: var(--bg-card); + border-radius: 1rem; + overflow: hidden; + box-shadow: var(--shadow-md); + transition: all 0.3s cubic-bezier(0.4, 0, 0.2, 1); + border: 1px solid var(--border-color); } .metric-card:hover { - transform: translateY(-4px); - box-shadow: var(--shadow-xl); + transform: translateY(-4px); + box-shadow: var(--shadow-xl); } .metric-header { - background: var(--accent-gradient); - color: white; - padding: 0.875rem 1rem; - font-size: 0.875rem; - font-weight: 600; - text-transform: uppercase; - letter-spacing: 0.05em; - text-align: center; + background: var(--accent-gradient); + color: white; + padding: 0.875rem 1rem; + font-size: 0.875rem; + font-weight: 600; + text-transform: uppercase; + letter-spacing: 0.05em; + text-align: center; } .metric-value { - padding: 1.5rem 1rem; - text-align: center; - background: var(--bg-card); - position: relative; - overflow: hidden; + padding: 1.5rem 1rem; + text-align: center; + background: var(--bg-card); + position: relative; + overflow: hidden; } .metric-value::before { - content: ''; - position: absolute; - top: 0; - left: 0; - right: 0; - height: 1px; - background: var(--accent-gradient); - opacity: 0.2; + content: ""; + position: absolute; + top: 0; + left: 0; + right: 0; + height: 1px; + background: var(--accent-gradient); + opacity: 0.2; } .metric-value span:first-child { - font-size: clamp(2rem, 5vw, 3rem); - font-weight: 700; - color: var(--text-primary); - display: inline-block; - transition: all 0.3s ease; + font-size: clamp(2rem, 5vw, 3rem); + font-weight: 700; + color: var(--text-primary); + display: inline-block; + transition: all 0.3s ease; } .metric-value.updated span:first-child { - animation: bounce 0.5s ease-out; - color: var(--accent-primary); + animation: bounce 0.5s ease-out; + color: var(--accent-primary); } .metric-unit { - font-size: clamp(0.875rem, 2vw, 1.25rem); - color: var(--text-muted); - font-weight: 400; - margin-left: 0.25rem; + font-size: clamp(0.875rem, 2vw, 1.25rem); + color: var(--text-muted); + font-weight: 400; + margin-left: 0.25rem; } /* Message Section */ .message-section { - background: var(--bg-card); - border-radius: 1rem; - padding: 1.5rem; - margin-bottom: 1.5rem; - box-shadow: var(--shadow-md); - border: 1px solid var(--border-color); + background: var(--bg-card); + border-radius: 1rem; + padding: 1.5rem; + margin-bottom: 1.5rem; + box-shadow: var(--shadow-md); + border: 1px solid var(--border-color); } .result-type { - font-size: clamp(1.25rem, 3vw, 1.75rem); - font-weight: 600; - color: var(--text-primary); - margin-bottom: 0.5rem; + font-size: clamp(1.25rem, 3vw, 1.75rem); + font-weight: 600; + color: var(--text-primary); + margin-bottom: 0.5rem; } .message { - color: var(--text-secondary); - font-size: clamp(0.875rem, 2vw, 1rem); - line-height: 1.5; + color: var(--text-secondary); + font-size: clamp(0.875rem, 2vw, 1rem); + line-height: 1.5; } /* Image Gallery */ .image-gallery { - background: var(--bg-card); - border-radius: 1rem; - padding: 1.5rem; - box-shadow: var(--shadow-md); - border: 1px solid var(--border-color); + background: var(--bg-card); + border-radius: 1rem; + padding: 1.5rem; + box-shadow: var(--shadow-md); + border: 1px solid var(--border-color); } .image-gallery h2 { - color: var(--text-primary); - margin-bottom: 1rem; - font-size: clamp(1.25rem, 3vw, 1.5rem); + color: var(--text-primary); + margin-bottom: 1rem; + font-size: clamp(1.25rem, 3vw, 1.5rem); } .image-grid { - display: grid; - grid-template-columns: repeat(auto-fit, minmax(250px, 1fr)); - gap: 1rem; + display: grid; + grid-template-columns: repeat(auto-fit, minmax(250px, 1fr)); + gap: 1rem; } .shot-image { - width: 100%; - height: auto; - border-radius: 0.5rem; - box-shadow: var(--shadow-sm); - transition: transform 0.3s ease; - cursor: pointer; + width: 100%; + height: auto; + border-radius: 0.5rem; + box-shadow: var(--shadow-sm); + transition: transform 0.3s ease; + cursor: pointer; } .shot-image:hover { - transform: scale(1.02); + transform: scale(1.02); } /* Ball Ready Indicator */ .ball-ready-indicator { - display: flex; - align-items: center; - gap: 1.5rem; - padding: 1.5rem; - margin-bottom: 2rem; - background: var(--bg-card); - border-radius: 1rem; - box-shadow: var(--shadow-lg); - transition: all 0.3s ease; + display: flex; + align-items: center; + gap: 1.5rem; + padding: 1.5rem; + margin-bottom: 2rem; + background: var(--bg-card); + border-radius: 1rem; + box-shadow: var(--shadow-lg); + transition: all 0.3s ease; } .ball-status-icon { - width: 80px; - height: 80px; - display: flex; - align-items: center; - justify-content: center; - transition: all 0.3s ease; + width: 80px; + height: 80px; + display: flex; + align-items: center; + justify-content: center; + transition: all 0.3s ease; } .ball-icon { - width: 100%; - height: 100%; - background-color: var(--text-muted); - -webkit-mask-image: url('/static/golf_ball.svg'); - -webkit-mask-size: contain; - -webkit-mask-repeat: no-repeat; - -webkit-mask-position: center; - mask-image: url('/static/golf_ball.svg'); - mask-size: contain; - mask-repeat: no-repeat; - mask-position: center; - transition: all 0.3s ease; + width: 100%; + height: 100%; + background-color: var(--text-muted); + -webkit-mask-image: url("/static/golf_ball.svg"); + -webkit-mask-size: contain; + -webkit-mask-repeat: no-repeat; + -webkit-mask-position: center; + mask-image: url("/static/golf_ball.svg"); + mask-size: contain; + mask-repeat: no-repeat; + mask-position: center; + transition: all 0.3s ease; } .ball-status-text { - flex: 1; + flex: 1; } .ball-status-title { - font-size: 1.25rem; - font-weight: 600; - color: var(--text-primary); - margin-bottom: 0.5rem; + font-size: 1.25rem; + font-weight: 600; + color: var(--text-primary); + margin-bottom: 0.5rem; } .ball-status-message { - font-size: 1rem; - color: var(--text-secondary); + font-size: 1rem; + color: var(--text-secondary); } /* Ball Status States */ .ball-ready-indicator.initializing { - border-left: 4px solid var(--info); + border-left: 4px solid var(--info); } .ball-ready-indicator.initializing .ball-icon { - background-color: var(--info); - animation: pulse 2s infinite; + background-color: var(--info); + animation: pulse 2s infinite; } .ball-ready-indicator.waiting { - border-left: 4px solid var(--warning); + border-left: 4px solid var(--warning); } .ball-ready-indicator.waiting .ball-icon { - background-color: var(--warning); - animation: pulse 2s infinite; + background-color: var(--warning); + animation: pulse 2s infinite; } .ball-ready-indicator.stabilizing { - border-left: 4px solid #fbbf24; + border-left: 4px solid #fbbf24; } .ball-ready-indicator.stabilizing .ball-icon { - background-color: #fbbf24; - animation: bounce 1s infinite; + background-color: #fbbf24; + animation: bounce 1s infinite; } .ball-ready-indicator.ready { - border-left: 4px solid var(--success); - background: linear-gradient(135deg, var(--bg-card) 0%, rgba(72, 187, 120, 0.1) 100%); + border-left: 4px solid var(--success); + background: linear-gradient( + 135deg, + var(--bg-card) 0%, + rgba(72, 187, 120, 0.1) 100% + ); } .ball-ready-indicator.ready .ball-icon { - background-color: var(--success); - transform: scale(1.1); + background-color: var(--success); + transform: scale(1.1); } .ball-ready-indicator.ready .ball-status-title { - color: var(--success); + color: var(--success); } .ball-ready-indicator.hit { - border-left: 4px solid var(--accent-primary); + border-left: 4px solid var(--accent-primary); } .ball-ready-indicator.hit .ball-icon { - background-color: var(--accent-primary); - animation: none; + background-color: var(--accent-primary); + animation: none; } .ball-ready-indicator.error { - border-left: 4px solid var(--error); + border-left: 4px solid var(--error); } .ball-ready-indicator.error .ball-icon { - background-color: var(--error); + background-color: var(--error); +} + +/* Open Golf Sim Controls */ +.sim-badge { + margin-top: 6px; + font-size: 12px; + opacity: 0.9; +} +.sim-badge__state { + font-weight: 700; + margin-left: 6px; } +.ogs-controls { + margin-top: 18px; + padding: 14px; + border-radius: 10px; + background: rgba(255, 255, 255, 0.03); +} +.ogs-controls h2 { + margin: 0 0 12px 0; + font-size: 16px; +} + +.ogs-controls-grid { + display: grid; + grid-template-columns: repeat(3, minmax(90px, 1fr)); + gap: 10px; + align-items: stretch; +} + +.ogs-btn { + padding: 14px 10px; + border-radius: 10px; + border: 1px solid rgba(255, 255, 255, 0.12); + background: rgba(255, 255, 255, 0.06); + color: inherit; + font-weight: 700; + cursor: pointer; + user-select: none; + touch-action: manipulation; +} + +.ogs-btn:active { + transform: scale(0.98); +} + +.ogs-controls-hint { + margin-top: 10px; + font-size: 12px; + opacity: 0.75; +} +/* Open Golf Sim Controls */ + @media (max-width: 768px) { - .metrics-grid { - grid-template-columns: 1fr; - gap: 1rem; - } + .metrics-grid { + grid-template-columns: 1fr; + gap: 1rem; + } - .metric-value { - padding: 1.25rem 0.75rem; - } + .metric-value { + padding: 1.25rem 0.75rem; + } } @media (min-width: 769px) and (max-width: 1024px) { - .metrics-grid { - grid-template-columns: repeat(2, 1fr); - } + .metrics-grid { + grid-template-columns: repeat(2, 1fr); + } } @media (min-width: 481px) and (max-width: 768px) { - .metrics-grid { - grid-template-columns: repeat(2, 1fr); - } + .metrics-grid { + grid-template-columns: repeat(2, 1fr); + } - .image-grid { - grid-template-columns: repeat(2, 1fr); - } + .image-grid { + grid-template-columns: repeat(2, 1fr); + } } @media (max-width: 480px) { - .ball-ready-indicator { - flex-direction: column; - text-align: center; - gap: 1rem; - padding: 1rem; - } - - .ball-status-icon { - width: 60px; - height: 60px; - } + .ball-ready-indicator { + flex-direction: column; + text-align: center; + gap: 1rem; + padding: 1rem; + } + + .ball-status-icon { + width: 60px; + height: 60px; + } } @media (min-width: 1400px) { - .metrics-grid { - grid-template-columns: repeat(3, 1fr); - } - - .metric-value span:first-child { - font-size: 3.5rem; - } -} \ No newline at end of file + .metrics-grid { + grid-template-columns: repeat(3, 1fr); + } + + .metric-value span:first-child { + font-size: 3.5rem; + } +}