From 3dfb1a80ab1fe325cac923acbcd6b6896ec0116b Mon Sep 17 00:00:00 2001 From: admercs Date: Thu, 25 Apr 2024 11:29:21 -0700 Subject: [PATCH] reorganized AutonomyLib C++ code --- AutonomyLib/include/api/ApiProvider.hpp | 14 +- AutonomyLib/include/api/ApiServerBase.hpp | 2 + .../include/api/RpcLibAdaptorsBase.hpp | 8 +- AutonomyLib/include/api/RpcLibClientBase.hpp | 18 +- AutonomyLib/include/api/RpcLibServerBase.hpp | 27 +- AutonomyLib/include/api/VehicleApiBase.hpp | 38 +- AutonomyLib/include/api/VehicleSimApiBase.hpp | 1 + AutonomyLib/include/api/WorldSimApiBase.hpp | 1 + .../include/common/AutonomySimSettings.hpp | 2179 +++--- AutonomyLib/include/common/CancelToken.hpp | 20 +- AutonomyLib/include/common/ClockBase.hpp | 14 +- AutonomyLib/include/common/ClockFactory.hpp | 10 +- AutonomyLib/include/common/Common.hpp | 4 +- AutonomyLib/include/common/CommonStructs.hpp | 2 +- AutonomyLib/include/common/DelayLine.hpp | 30 +- AutonomyLib/include/common/EarthCelestial.hpp | 165 +- AutonomyLib/include/common/EarthUtils.hpp | 93 +- .../include/common/FirstOrderFilter.hpp | 36 +- .../include/common/FrequencyLimiter.hpp | 27 +- AutonomyLib/include/common/GaussianMarkov.hpp | 18 +- .../include/common/GeodeticConverter.hpp | 96 +- .../include/common/ImageCaptureBase.hpp | 13 +- AutonomyLib/include/common/LogFileWriter.hpp | 14 +- AutonomyLib/include/common/PidController.hpp | 1 + AutonomyLib/include/common/ScalableClock.hpp | 35 +- AutonomyLib/include/common/Settings.hpp | 8 +- AutonomyLib/include/common/StateReporter.hpp | 16 +- .../include/common/StateReporterWrapper.hpp | 32 +- AutonomyLib/include/common/SteppableClock.hpp | 13 +- .../include/common/UpdatableContainer.hpp | 10 +- .../include/common/UpdatableObject.hpp | 25 +- AutonomyLib/include/common/VectorMath.hpp | 29 +- AutonomyLib/include/common/Waiter.hpp | 29 +- AutonomyLib/include/common/WorkerThread.hpp | 171 +- .../include/common/utils/AsyncTasker.hpp | 10 +- .../include/common/utils/ColorUtils.hpp | 1 + .../include/common/utils/EnumFlags.hpp | 1 + .../include/common/utils/FileSystem.hpp | 4 + .../include/common/utils/MedianFilter.hpp | 1 + .../include/common/utils/OnlineStats.hpp | 9 +- .../include/common/utils/ProsumerQueue.hpp | 45 +- .../include/common/utils/RandomGenerator.hpp | 9 +- .../common/utils/ScheduledExecutor.hpp | 195 +- AutonomyLib/include/common/utils/Signal.hpp | 8 +- .../include/common/utils/SmoothingFilter.hpp | 1 + AutonomyLib/include/common/utils/Timer.hpp | 44 +- .../include/common/utils/UniqueValueMap.hpp | 11 +- AutonomyLib/include/common/utils/Utils.hpp | 23 +- AutonomyLib/include/common/utils/json.hpp | 6767 ++++++++++++----- AutonomyLib/include/common/utils/sincos.hpp | 3 + .../include/common/utils/type_utils.hpp | 4 + .../include/physics/DebugPhysicsBody.hpp | 111 +- AutonomyLib/include/physics/Environment.hpp | 61 +- .../include/physics/ExternalPhysicsEngine.hpp | 2 + .../include/physics/FastPhysicsEngine.hpp | 107 +- AutonomyLib/include/physics/Kinematics.hpp | 9 +- AutonomyLib/include/physics/PhysicsBody.hpp | 59 +- .../include/physics/PhysicsBodyVertex.hpp | 15 +- .../include/physics/PhysicsEngineBase.hpp | 1 + AutonomyLib/include/physics/World.hpp | 43 +- AutonomyLib/include/safety/CubeGeoFence.hpp | 6 +- AutonomyLib/include/safety/IGeoFence.hpp | 1 + AutonomyLib/include/safety/ObstacleMap.hpp | 13 +- AutonomyLib/include/safety/SafetyEval.hpp | 36 +- AutonomyLib/include/safety/SphereGeoFence.hpp | 1 + AutonomyLib/include/sensors/SensorBase.hpp | 16 +- .../include/sensors/SensorCollection.hpp | 13 +- AutonomyLib/include/sensors/SensorFactory.hpp | 6 +- .../sensors/barometer/BarometerBase.hpp | 16 +- .../sensors/barometer/BarometerSimple.hpp | 77 +- .../barometer/BarometerSimpleParams.hpp | 19 +- .../include/sensors/distance/DistanceBase.hpp | 14 +- .../sensors/distance/DistanceSimple.hpp | 64 +- .../sensors/distance/DistanceSimpleParams.hpp | 19 +- AutonomyLib/include/sensors/gps/GpsBase.hpp | 15 +- AutonomyLib/include/sensors/gps/GpsSimple.hpp | 63 +- .../include/sensors/gps/GpsSimpleParams.hpp | 1 + AutonomyLib/include/sensors/imu/ImuBase.hpp | 15 +- AutonomyLib/include/sensors/imu/ImuSimple.hpp | 74 +- .../include/sensors/imu/ImuSimpleParams.hpp | 18 +- .../include/sensors/lidar/LidarBase.hpp | 14 +- .../include/sensors/lidar/LidarSimple.hpp | 89 +- .../sensors/magnetometer/MagnetometerBase.hpp | 20 +- .../magnetometer/MagnetometerSimple.hpp | 89 +- .../magnetometer/MagnetometerSimpleParams.hpp | 1 + .../include/vehicles/car/CarApiFactory.hpp | 1 + .../include/vehicles/car/api/CarApiBase.hpp | 14 +- .../vehicles/car/api/CarRpcLibAdaptors.hpp | 1 + .../vehicles/car/api/CarRpcLibClient.hpp | 2 + .../vehicles/car/api/CarRpcLibServer.hpp | 8 +- .../car/firmwares/ardurover/ArduRoverApi.hpp | 174 +- .../car/firmwares/physxcar/PhysXCarApi.hpp | 21 +- .../vehicles/multirotor/MultiRotorParams.hpp | 125 +- .../multirotor/MultiRotorParamsFactory.hpp | 1 + .../multirotor/MultiRotorPhysicsBody.hpp | 145 +- .../vehicles/multirotor/RotorActuator.hpp | 77 +- .../vehicles/multirotor/RotorParams.hpp | 25 +- .../multirotor/api/MultirotorApiBase.hpp | 316 +- .../multirotor/api/MultirotorCommon.hpp | 5 +- .../api/MultirotorRpcLibAdaptors.hpp | 1 + .../multirotor/api/MultirotorRpcLibClient.hpp | 9 +- .../multirotor/api/MultirotorRpcLibServer.hpp | 7 +- .../firmwares/arducopter/ArduCopterApi.hpp | 437 +- .../firmwares/arducopter/ArduCopterParams.hpp | 29 +- .../firmwares/mavlink/ArduCopterSoloApi.hpp | 225 +- .../mavlink/ArduCopterSoloParams.hpp | 57 +- .../mavlink/MavLinkMultirotorApi.hpp | 3040 ++++---- .../firmwares/mavlink/Px4MultiRotorParams.hpp | 26 +- .../simple_flight/SimpleFlightApi.hpp | 213 +- .../simple_flight/SimpleFlightBoard.hpp | 33 +- .../simple_flight/SimpleFlightCommLink.hpp | 13 +- .../simple_flight/SimpleFlightCommon.hpp | 1 + .../simple_flight/SimpleFlightEstimator.hpp | 9 +- .../simple_flight/SimpleFlightQuadXParams.hpp | 27 +- .../firmware/AdaptiveController.hpp | 112 +- .../firmware/AngleLevelController.hpp | 85 +- .../firmware/AngleRateController.hpp | 24 +- .../firmware/CascadeController.hpp | 34 +- .../firmware/ConstantOutputController.hpp | 14 +- .../simple_flight/firmware/Firmware.hpp | 27 +- .../simple_flight/firmware/Mixer.hpp | 45 +- .../simple_flight/firmware/OffboardApi.hpp | 125 +- .../simple_flight/firmware/Params.hpp | 1 + .../firmware/PassthroughController.hpp | 12 +- .../simple_flight/firmware/PidController.hpp | 31 +- .../firmware/PositionController.hpp | 32 +- .../simple_flight/firmware/RemoteControl.hpp | 199 +- .../firmware/RungKuttaPidIntegrator.hpp | 67 +- .../firmware/StdPidIntegrator.hpp | 20 +- .../firmware/VelocityController.hpp | 32 +- .../firmware/interfaces/CommonStructs.hpp | 29 +- .../firmware/interfaces/IAxisController.hpp | 2 + .../firmware/interfaces/IBoardClock.hpp | 1 + .../firmware/interfaces/IBoardInputPins.hpp | 1 + .../firmware/interfaces/IBoardOutputPins.hpp | 1 + .../firmware/interfaces/IBoardSensors.hpp | 1 + .../firmware/interfaces/ICommLink.hpp | 2 + .../firmware/interfaces/IController.hpp | 2 + .../firmware/interfaces/IFirmware.hpp | 1 + .../firmware/interfaces/IGoal.hpp | 1 + .../firmware/interfaces/IOffboardApi.hpp | 2 + .../firmware/interfaces/IPidIntegrator.hpp | 5 +- .../firmware/interfaces/IStateEstimator.hpp | 1 + .../firmware/interfaces/IUpdatable.hpp | 22 +- AutonomyLib/src/api/RpcLibClientBase.cpp | 2 +- AutonomyLib/src/api/RpcLibServerBase.cpp | 2 +- AutonomyLib/src/common/utils/FileSystem.cpp | 1 + AutonomyLib/src/safety/ObstacleMap.cpp | 1 + AutonomyLib/src/safety/SafetyEval.cpp | 1 + .../src/vehicles/car/api/CarRpcLibClient.cpp | 2 +- .../src/vehicles/car/api/CarRpcLibServer.cpp | 2 - .../multirotor/api/MultirotorApiBase.cpp | 1 + .../multirotor/api/MultirotorRpcLibClient.cpp | 3 +- .../multirotor/api/MultirotorRpcLibServer.cpp | 2 - AutonomyLibUnitTests/CelestialTests.hpp | 1 + AutonomyLibUnitTests/PixhawkTest.hpp | 1 + AutonomyLibUnitTests/QuaternionTest.hpp | 13 +- AutonomyLibUnitTests/SettingsTest.hpp | 1 + AutonomyLibUnitTests/SimpleFlightTest.hpp | 7 +- AutonomyLibUnitTests/TestBase.hpp | 2 + AutonomyLibUnitTests/WorkerThreadTest.hpp | 10 +- 161 files changed, 10147 insertions(+), 7321 deletions(-) diff --git a/AutonomyLib/include/api/ApiProvider.hpp b/AutonomyLib/include/api/ApiProvider.hpp index 169a5c08..71557889 100644 --- a/AutonomyLib/include/api/ApiProvider.hpp +++ b/AutonomyLib/include/api/ApiProvider.hpp @@ -8,12 +8,20 @@ #include "VehicleSimApiBase.hpp" #include "WorldSimApiBase.hpp" #include "common/utils/UniqueValueMap.hpp" + #include namespace nervosys { namespace autonomylib { class ApiProvider { + + private: + WorldSimApiBase *world_sim_api_; + + common_utils::UniqueValueMap vehicle_apis_; + common_utils::UniqueValueMap vehicle_sim_apis_; + public: ApiProvider(WorldSimApiBase *world_sim_api) : world_sim_api_(world_sim_api) {} virtual ~ApiProvider() = default; @@ -54,12 +62,6 @@ class ApiProvider { vehicle_apis_.insert_or_assign("", vehicle_apis_.at(vehicle_name)); vehicle_sim_apis_.insert_or_assign("", vehicle_sim_apis_.at(vehicle_name)); } - - private: - WorldSimApiBase *world_sim_api_; - - common_utils::UniqueValueMap vehicle_apis_; - common_utils::UniqueValueMap vehicle_sim_apis_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/api/ApiServerBase.hpp b/AutonomyLib/include/api/ApiServerBase.hpp index 49eec51e..b69b1956 100644 --- a/AutonomyLib/include/api/ApiServerBase.hpp +++ b/AutonomyLib/include/api/ApiServerBase.hpp @@ -5,12 +5,14 @@ #define autonomylib_api_ApiServerBase_hpp #include "common/Common.hpp" + #include namespace nervosys { namespace autonomylib { class ApiServerBase { + public: virtual void start(bool block, std::size_t thread_count) = 0; virtual void stop() = 0; diff --git a/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp b/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp index e285f24f..5411d6d6 100644 --- a/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp +++ b/AutonomyLib/include/api/RpcLibAdaptorsBase.hpp @@ -8,18 +8,18 @@ #include "common/Common.hpp" #include "common/CommonStructs.hpp" #include "common/ImageCaptureBase.hpp" -#include "physics/Environment.hpp" -#include "physics/Kinematics.hpp" -#include "safety/SafetyEval.hpp" - #include "common/utils/WindowsApisCommonPost.hpp" #include "common/utils/WindowsApisCommonPre.hpp" +#include "physics/Environment.hpp" +#include "physics/Kinematics.hpp" #include "rpc/msgpack.hpp" +#include "safety/SafetyEval.hpp" namespace nervosys { namespace autonomylib_rpclib { class RpcLibAdaptorsBase { + public: template static void to(const std::vector &s, std::vector &d) { d.clear(); diff --git a/AutonomyLib/include/api/RpcLibClientBase.hpp b/AutonomyLib/include/api/RpcLibClientBase.hpp index 1cc3665f..d4d330aa 100644 --- a/AutonomyLib/include/api/RpcLibClientBase.hpp +++ b/AutonomyLib/include/api/RpcLibClientBase.hpp @@ -21,10 +21,18 @@ namespace autonomylib { // common methods for RCP clients of different vehicles class RpcLibClientBase { + + private: + struct impl; + std::unique_ptr pimpl_; + + protected: + void *getClient(); + const void *getClient() const; + public: enum class ConnectionState : uint { Initial = 0, Connected, Disconnected, Reset, Unknown }; - public: RpcLibClientBase(const string &ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60); virtual ~RpcLibClientBase(); // required for pimpl @@ -208,14 +216,6 @@ class RpcLibClientBase { std::string getSettingsString() const; std::vector simListAssets() const; - - protected: - void *getClient(); - const void *getClient() const; - - private: - struct impl; - std::unique_ptr pimpl_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/api/RpcLibServerBase.hpp b/AutonomyLib/include/api/RpcLibServerBase.hpp index 0171d50d..98ddcae3 100644 --- a/AutonomyLib/include/api/RpcLibServerBase.hpp +++ b/AutonomyLib/include/api/RpcLibServerBase.hpp @@ -12,17 +12,12 @@ namespace nervosys { namespace autonomylib { class RpcLibServerBase : public ApiServerBase { - public: - RpcLibServerBase(ApiProvider *api_provider, const std::string &server_address, uint16_t port = RpcLibPort); - virtual ~RpcLibServerBase() override; - virtual void start(bool block, std::size_t thread_count) override; - virtual void stop() override; + private: + ApiProvider *api_provider_; - class ApiNotSupported : public std::runtime_error { - public: - ApiNotSupported(const std::string &message) : std::runtime_error(message) {} - }; + struct impl; + std::unique_ptr pimpl_; protected: void *getServer() const; @@ -54,11 +49,17 @@ class RpcLibServerBase : public ApiServerBase { "' is not available. This could be because this is not a simulation"); } - private: - ApiProvider *api_provider_; + public: + RpcLibServerBase(ApiProvider *api_provider, const std::string &server_address, uint16_t port = RpcLibPort); + virtual ~RpcLibServerBase() override; - struct impl; - std::unique_ptr pimpl_; + virtual void start(bool block, std::size_t thread_count) override; + virtual void stop() override; + + class ApiNotSupported : public std::runtime_error { + public: + ApiNotSupported(const std::string &message) : std::runtime_error(message) {} + }; }; } // namespace autonomylib diff --git a/AutonomyLib/include/api/VehicleApiBase.hpp b/AutonomyLib/include/api/VehicleApiBase.hpp index 28f1f8a4..4e8dd5b9 100644 --- a/AutonomyLib/include/api/VehicleApiBase.hpp +++ b/AutonomyLib/include/api/VehicleApiBase.hpp @@ -17,6 +17,7 @@ #include "sensors/imu/ImuBase.hpp" #include "sensors/lidar/LidarBase.hpp" #include "sensors/magnetometer/MagnetometerBase.hpp" + #include #include @@ -31,6 +32,25 @@ The base class defines usually available methods that all vehicle controllers ma Some methods may not be applicable to specific vehicle in which case an exception may be raised or call may be ignored. */ class VehicleApiBase : public UpdatableObject { + + private: + const SensorBase *findSensorByName(const std::string &sensor_name, const SensorBase::SensorType type) const { + const SensorBase *sensor = nullptr; + + // Find sensor with the given name (for empty input name, return the first one found) + // Not efficient but should suffice given small number of sensors + uint count_sensors = getSensors().size(type); + for (uint i = 0; i < count_sensors; i++) { + const SensorBase *current_sensor = getSensors().getByType(type, i); + if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) { + sensor = current_sensor; + break; + } + } + + return sensor; + } + public: virtual void enableApiControl(bool is_enabled) = 0; virtual bool isApiControlEnabled() const = 0; @@ -173,24 +193,6 @@ class VehicleApiBase : public UpdatableObject { public: VehicleMoveException(const std::string &message) : VehicleControllerException(message) {} }; - - private: - const SensorBase *findSensorByName(const std::string &sensor_name, const SensorBase::SensorType type) const { - const SensorBase *sensor = nullptr; - - // Find sensor with the given name (for empty input name, return the first one found) - // Not efficient but should suffice given small number of sensors - uint count_sensors = getSensors().size(type); - for (uint i = 0; i < count_sensors; i++) { - const SensorBase *current_sensor = getSensors().getByType(type, i); - if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) { - sensor = current_sensor; - break; - } - } - - return sensor; - } }; } // namespace autonomylib diff --git a/AutonomyLib/include/api/VehicleSimApiBase.hpp b/AutonomyLib/include/api/VehicleSimApiBase.hpp index 5284b9ec..40bd93ee 100644 --- a/AutonomyLib/include/api/VehicleSimApiBase.hpp +++ b/AutonomyLib/include/api/VehicleSimApiBase.hpp @@ -15,6 +15,7 @@ namespace nervosys { namespace autonomylib { class VehicleSimApiBase : public nervosys::autonomylib::UpdatableObject { + public: virtual ~VehicleSimApiBase() = default; diff --git a/AutonomyLib/include/api/WorldSimApiBase.hpp b/AutonomyLib/include/api/WorldSimApiBase.hpp index 059e9d20..24251b47 100644 --- a/AutonomyLib/include/api/WorldSimApiBase.hpp +++ b/AutonomyLib/include/api/WorldSimApiBase.hpp @@ -11,6 +11,7 @@ namespace nervosys { namespace autonomylib { class WorldSimApiBase { + public: enum class WeatherParameter { Rain = 0, diff --git a/AutonomyLib/include/common/AutonomySimSettings.hpp b/AutonomyLib/include/common/AutonomySimSettings.hpp index 92dda092..e3b661b8 100644 --- a/AutonomyLib/include/common/AutonomySimSettings.hpp +++ b/AutonomyLib/include/common/AutonomySimSettings.hpp @@ -13,1322 +13,1323 @@ #include "CommonStructs.hpp" #include "ImageCaptureBase.hpp" #include "Settings.hpp" -#include "common/utils/Utils.hpp" #include "sensors/SensorBase.hpp" +#include "utils/Utils.hpp" namespace nervosys { namespace autonomylib { struct AutonomySimSettings { + private: typedef common_utils::Utils Utils; typedef ImageCaptureBase::ImageType ImageType; - public: // types - static constexpr int kSubwindowCount = 3; // must be >= 3 for now - static constexpr char const *kVehicleTypePX4 = "px4multirotor"; - static constexpr char const *kVehicleTypeArduCopterSolo = "arducoptersolo"; - static constexpr char const *kVehicleTypeSimpleFlight = "simpleflight"; - static constexpr char const *kVehicleTypeArduCopter = "arducopter"; - static constexpr char const *kVehicleTypePhysXCar = "physxcar"; - static constexpr char const *kVehicleTypeArduRover = "ardurover"; - static constexpr char const *kVehicleTypeComputerVision = "computervision"; - - static constexpr char const *kVehicleInertialFrame = "VehicleInertialFrame"; - static constexpr char const *kSensorLocalFrame = "SensorLocalFrame"; + // fields + float settings_version_actual; + float settings_version_minimum = 1.2f; - static constexpr char const *kSimModeTypeMultirotor = "Multirotor"; - static constexpr char const *kSimModeTypeCar = "Car"; - static constexpr char const *kSimModeTypeComputerVision = "ComputerVision"; + void checkSettingsVersion(const Settings &settings_json) { + bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual); + bool upgrade_required = settings_version_actual < settings_version_minimum; + if (upgrade_required) { + bool auto_upgrade = false; - struct SubwindowSetting { - int window_index; - ImageType image_type; - bool visible; - std::string camera_name; - std::string vehicle_name; - bool external; + // if we have default setting file not modified by user then we will + // just auto-upgrade it + if (has_default_settings) { + auto_upgrade = true; + } else { + // check if auto-upgrade is possible + if (settings_version_actual == 1) { + const std::vector all_changed_keys = {"AdditionalCameras", "CaptureSettings", + "NoiseSettings", "UsageScenario", + "SimpleFlight", "PX4"}; + std::stringstream detected_keys_ss; + for (const auto &changed_key : all_changed_keys) { + if (settings_json.hasKey(changed_key)) + detected_keys_ss << changed_key << ","; + } + std::string detected_keys = detected_keys_ss.str(); + if (detected_keys.length()) { + std::string error_message = + "You are using newer version of AutonomySim with older version of settings.json. " + "You can either delete your settings.json and restart AutonomySim or use the upgrade " + "instructions at https://git.io/vjefh. \n\n" + "Following keys in your settings.json needs updating: "; - SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, - bool visible_val = false, const std::string &camera_name_val = "", - const std::string &vehicle_name_val = "", bool external_val = false) - : window_index(window_index_val), image_type(image_type_val), visible(visible_val), - camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) {} - }; + error_messages.push_back(error_message + detected_keys); + } else + auto_upgrade = true; + } else + auto_upgrade = true; + } - struct RecordingSetting { - bool record_on_move = false; - float record_interval = 0.05f; - std::string folder = ""; - bool enabled = false; + if (auto_upgrade) { + warning_messages.push_back( + "You are using newer version of AutonomySim with older version of settings.json. " + "You should delete your settings.json file and restart AutonomySim."); + } + } + // else no action necessary + } - std::map> requests; + bool hasDefaultSettings(const Settings &settings_json, float &version) { + // if empty settings file + bool has_default = settings_json.size() == 0; - RecordingSetting() {} + bool has_docs = + settings_json.getString("SeeDocsAt", "") != "" || settings_json.getString("see_docs_at", "") != ""; + // we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :( + version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0)); - RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string &folder_val, - bool enabled_val) - : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), - enabled(enabled_val) {} - }; + // If we have pre-V1 settings and only element is docs link + has_default |= settings_json.size() == 1 && has_docs; - struct PawnPath { - std::string pawn_bp; - std::string slippery_mat; - std::string non_slippery_mat; + // if we have V1 settings and only elements are docs link and version + has_default |= settings_json.size() == 2 && has_docs && version > 0; - PawnPath(const std::string &pawn_bp_val = "", - const std::string &slippery_mat_val = "/AutonomySim/VehicleAdv/PhysicsMaterials/Slippery.Slippery", - const std::string &non_slippery_mat_val = - "/AutonomySim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") - : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) {} - }; + return has_default; + } - struct RCSettings { - int remote_control_id = -1; - bool allow_api_when_disconnected = false; - }; + void loadCoreSimModeSettings(const Settings &settings_json, std::function simmode_getter) { + // get the simmode from user if not specified + simmode_name = settings_json.getString("SimMode", ""); + if (simmode_name == "") { + if (simmode_getter) + simmode_name = simmode_getter(); + else + throw std::invalid_argument("simmode_name is not expected empty in SimModeBase"); + } - struct Rotation { - float yaw = 0; - float pitch = 0; - float roll = 0; + physics_engine_name = settings_json.getString("PhysicsEngineName", ""); + if (physics_engine_name == "") { + if (simmode_name == kSimModeTypeMultirotor) + physics_engine_name = "FastPhysicsEngine"; + else + physics_engine_name = "PhysX"; // this value is only informational for now + } + } - Rotation() {} + void loadLevelSettings(const Settings &settings_json) { + level_name = settings_json.getString("Default Environment", ""); + } - Rotation(float yaw_val, float pitch_val, float roll_val) : yaw(yaw_val), pitch(pitch_val), roll(roll_val) {} + void loadViewModeSettings(const Settings &settings_json) { + std::string view_mode_string = settings_json.getString("ViewMode", ""); - static Rotation nanRotation() noexcept { - static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); - return val; + if (view_mode_string == "") { + if (simmode_name == kSimModeTypeMultirotor) + view_mode_string = "FlyWithMe"; + else if (simmode_name == kSimModeTypeComputerVision) + view_mode_string = "Fpv"; + else + view_mode_string = "SpringArmChase"; } - }; - - struct GimbalSetting { - float stabilization = 0; - // bool is_world_frame = false; - Rotation rotation = Rotation::nanRotation(); - }; - struct CaptureSetting { - // below settings_json are obtained by using Unreal console command (press ~): - // ShowFlag.VisualizeHDR 1. - // to replicate camera settings_json to SceneCapture2D - // TODO: should we use UAutonomyBlueprintLib::GetDisplayGamma()? - static constexpr float kSceneTargetGamma = 1.4f; + if (view_mode_string == "Fpv") + initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; + else if (view_mode_string == "GroundObserver") + initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; + else if (view_mode_string == "FlyWithMe") + initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; + else if (view_mode_string == "Manual") + initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; + else if (view_mode_string == "SpringArmChase") + initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; + else if (view_mode_string == "Backup") + initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; + else if (view_mode_string == "NoDisplay") + initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; + else if (view_mode_string == "Front") + initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; + else + error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); + } - int image_type = 0; + static void loadRCSetting(const std::string &simmode_name, const Settings &settings_json, RCSettings &rc_setting) { + Settings rc_json; + if (settings_json.getChild("RC", rc_json)) { + rc_setting.remote_control_id = + rc_json.getInt("RemoteControlID", simmode_name == kSimModeTypeMultirotor ? 0 : -1); + rc_setting.allow_api_when_disconnected = + rc_json.getBool("AllowAPIWhenDisconnected", rc_setting.allow_api_when_disconnected); + } + } - unsigned int width = 256, height = 144; // 960 X 540 - float fov_degrees = Utils::nan(); // 90.0f - int auto_exposure_method = -1; // histogram - float auto_exposure_speed = Utils::nan(); // 100.0f; - float auto_exposure_bias = Utils::nan(); // 0; - float auto_exposure_max_brightness = Utils::nan(); // 0.64f; - float auto_exposure_min_brightness = Utils::nan(); // 0.03f; - float auto_exposure_low_percent = Utils::nan(); // 80.0f; - float auto_exposure_high_percent = Utils::nan(); // 98.3f; - float auto_exposure_histogram_log_min = Utils::nan(); // -8; - float auto_exposure_histogram_log_max = Utils::nan(); // 4; - float motion_blur_amount = Utils::nan(); - float target_gamma = - Utils::nan(); // 1.0f; //This would be reset to kSceneTargetGamma for scene as default - int projection_mode = 0; // ECameraProjectionMode::Perspective - float ortho_width = Utils::nan(); - }; + static std::string getCameraName(const Settings &settings_json) { + return settings_json.getString( + "CameraName", + // TODO: below exist only due to legacy reason and can be replaced by "" in future + std::to_string(settings_json.getInt("CameraID", 0))); + } - struct NoiseSetting { - int ImageType = 0; + void loadDefaultRecordingSettings() { + recording_setting.requests.clear(); + // Add Scene image for each vehicle + for (const auto &vehicle : vehicles) { + recording_setting.requests[vehicle.first].push_back( + ImageCaptureBase::ImageRequest("", ImageType::Scene, false, true)); + } + } - bool Enabled = false; + void loadRecordingSetting(const Settings &settings_json) { + loadDefaultRecordingSettings(); - float RandContrib = 0.2f; - float RandSpeed = 100000.0f; - float RandSize = 500.0f; - float RandDensity = 2.0f; + Settings recording_json; + if (settings_json.getChild("Recording", recording_json)) { + recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move); + recording_setting.record_interval = + recording_json.getFloat("RecordInterval", recording_setting.record_interval); + recording_setting.folder = recording_json.getString("Folder", recording_setting.folder); + recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled); - float HorzWaveContrib = 0.03f; - float HorzWaveStrength = 0.08f; - float HorzWaveVertSize = 1.0f; - float HorzWaveScreenSize = 1.0f; + Settings req_cameras_settings; + if (recording_json.getChild("Cameras", req_cameras_settings)) { + // If 'Cameras' field is present, clear defaults + recording_setting.requests.clear(); + // Get name of the default vehicle to be used if "VehicleName" isn't specified + // Map contains a default vehicle if vehicles haven't been specified + std::string default_vehicle_name = vehicles.begin()->first; - float HorzNoiseLinesContrib = 1.0f; - float HorzNoiseLinesDensityY = 0.01f; - float HorzNoiseLinesDensityXY = 0.5f; + for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { + Settings req_camera_settings; - float HorzDistortionContrib = 1.0f; - float HorzDistortionStrength = 0.002f; - }; - - struct PixelFormatOverrideSetting { - int pixel_format = 0; - }; - - struct UnrealEngineSetting { - std::map pixel_format_override_settings; - }; - - using CaptureSettingsMap = std::map; - using NoiseSettingsMap = std::map; - struct CameraSetting { - // nan means keep the default values set in components - Vector3r position = VectorMath::nanVector(); - Rotation rotation = Rotation::nanRotation(); - - GimbalSetting gimbal; - CaptureSettingsMap capture_settings; - NoiseSettingsMap noise_settings; - - UnrealEngineSetting ue_setting; + if (req_cameras_settings.getChild(child_index, req_camera_settings)) { + std::string camera_name = getCameraName(req_camera_settings); + ImageType image_type = Utils::toEnum(req_camera_settings.getInt("ImageType", 0)); + bool compress = req_camera_settings.getBool("Compress", true); + bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false); + std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name); - CameraSetting() { - initializeCaptureSettings(capture_settings); - initializeNoiseSettings(noise_settings); + recording_setting.requests[vehicle_name].push_back( + ImageCaptureBase::ImageRequest(camera_name, image_type, pixels_as_float, compress)); + } + } + } } - }; - using CameraSettingMap = std::map; - - struct CameraDirectorSetting { - Vector3r position = VectorMath::nanVector(); - Rotation rotation = Rotation::nanRotation(); - float follow_distance = Utils::nan(); - }; - - struct SensorSetting { - SensorBase::SensorType sensor_type; - std::string sensor_name; - bool enabled = true; - Settings settings; // imported json data that needs to be parsed by specific sensors. - }; - - struct BarometerSetting : SensorSetting {}; + } - struct ImuSetting : SensorSetting {}; + static void initializeCaptureSettings(CaptureSettingsMap &capture_settings) { + capture_settings.clear(); + for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) { + capture_settings[i] = CaptureSetting(); + } + capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; + } - struct GpsSetting : SensorSetting {}; + static void loadCaptureSettings(const Settings &settings_json, CaptureSettingsMap &capture_settings) { + // We don't call initializeCaptureSettings here since it's already called in CameraSettings constructor + // And to avoid overwriting any defaults already set from CameraDefaults - struct MagnetometerSetting : SensorSetting {}; + Settings json_parent; + if (settings_json.getChild("CaptureSettings", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + CaptureSetting capture_setting; + createCaptureSettings(json_settings_child, capture_setting); + capture_settings[capture_setting.image_type] = capture_setting; + } + } + } + } - struct DistanceSetting : SensorSetting {}; + static std::unique_ptr createMavLinkVehicleSetting(const Settings &settings_json) { + // these settings_json are expected in same section, not in another child + std::unique_ptr vehicle_setting_p = + std::unique_ptr(new MavLinkVehicleSetting()); + MavLinkVehicleSetting *vehicle_setting = static_cast(vehicle_setting_p.get()); - struct LidarSetting : SensorSetting { - enum class DataFrame { VehicleInertialFrame, SensorLocalFrame }; - }; + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; - struct VehicleSetting { - // required - std::string vehicle_name; - std::string vehicle_type; + MavLinkConnectionInfo &connection_info = vehicle_setting->connection_info; + connection_info.sim_sysid = static_cast(settings_json.getInt("SimSysID", connection_info.sim_sysid)); + connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid); - // optional - std::string default_vehicle_state; - std::string pawn_path; - bool allow_api_always = true; - bool auto_create = true; - bool enable_collision_passthrough = false; - bool enable_trace = false; - bool enable_collisions = true; - bool is_fpv_vehicle = false; + connection_info.vehicle_sysid = + static_cast(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid)); + connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid); - // nan means use player start - Vector3r position = VectorMath::nanVector(); // in global NED - Rotation rotation = Rotation::nanRotation(); + connection_info.offboard_sysid = + static_cast(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid)); + connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid); - CameraSettingMap cameras; - std::map> sensors; + connection_info.logviewer_ip_address = + settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address); + connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port); + connection_info.logviewer_ip_sport = + settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport); - RCSettings rc; + connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address); + connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port); - VehicleSetting() {} + connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); + connection_info.control_port_local = + settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy + connection_info.control_port_local = + settings_json.getInt("ControlPortLocal", connection_info.control_port_local); + connection_info.control_port_remote = + settings_json.getInt("ControlPortRemote", connection_info.control_port_remote); - VehicleSetting(const std::string &vehicle_name_val, const std::string &vehicle_type_val) - : vehicle_name(vehicle_name_val), vehicle_type(vehicle_type_val) {} - }; + std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address); + if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) { + // backwards compat + connection_info.control_ip_address = sitlip; + } + if (settings_json.hasKey("SitlPort")) { + // backwards compat + connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local); + } - struct MavLinkConnectionInfo { - /* Default values are requires so uninitialized instance doesn't have random values */ + connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); - bool use_serial = true; // false means use UDP or TCP instead + connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial); + connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address); + connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port); + connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp); + connection_info.lock_step = settings_json.getBool("LockStep", connection_info.lock_step); + connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port); + connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port); + connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate); + connection_info.model = settings_json.getString("Model", connection_info.model); + connection_info.logs = settings_json.getString("Logs", connection_info.logs); - // Used to connect via HITL: needed only if use_serial = true - std::string serial_port = "*"; - int baud_rate = 115200; + Settings params; + if (settings_json.getChild("Parameters", params)) { + std::vector keys; + params.getChildNames(keys); + for (auto key : keys) { + connection_info.params[key] = params.getFloat(key, 0); + } + } - // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false - std::string udp_address = "127.0.0.1"; - int udp_port = 14560; + return vehicle_setting_p; + } - // Used to accept connections from drone over TCP: needed only if use_tcp = true - bool lock_step = true; - bool use_tcp = false; - int tcp_port = 4560; + static std::unique_ptr + createVehicleSetting(const std::string &simmode_name, const Settings &settings_json, const std::string vehicle_name, + std::map> &sensor_defaults, + const CameraSetting &camera_defaults) { + auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); - // The PX4 SITL app requires receiving drone commands over a different mavlink channel called - // the "ground control station" (or GCS) channel. - std::string control_ip_address = "127.0.0.1"; - int control_port_local = 14540; - int control_port_remote = 14580; + std::unique_ptr vehicle_setting; + if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || + vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) + vehicle_setting = createMavLinkVehicleSetting(settings_json); + // for everything else we don't need derived class yet + else { + vehicle_setting = std::unique_ptr(new VehicleSetting()); + if (vehicle_type == kVehicleTypeSimpleFlight) { + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + vehicle_setting->rc.remote_control_id = 0; + } + } + vehicle_setting->vehicle_name = vehicle_name; - // The log viewer can be on a different machine, so you can configure it's ip address and port here. - int logviewer_ip_port = 14388; - int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle. - std::string logviewer_ip_address = ""; + // required settings_json + vehicle_setting->vehicle_type = vehicle_type; - // The QGroundControl app can be on a different machine, and AutonomySim can act as a proxy to forward - // the mavlink stream over to that machine if you configure it's ip address and port here. - int qgc_ip_port = 0; - std::string qgc_ip_address = ""; + // optional settings_json + vehicle_setting->pawn_path = settings_json.getString("PawnPath", ""); + vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", ""); + vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways", vehicle_setting->allow_api_always); + vehicle_setting->auto_create = settings_json.getBool("AutoCreate", vehicle_setting->auto_create); + vehicle_setting->enable_collision_passthrough = + settings_json.getBool("EnableCollisionPassthrogh", vehicle_setting->enable_collision_passthrough); + vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", vehicle_setting->enable_trace); + vehicle_setting->enable_collisions = + settings_json.getBool("EnableCollisions", vehicle_setting->enable_collisions); + vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle", vehicle_setting->is_fpv_vehicle); - // mavlink vehicle identifiers - uint8_t sim_sysid = 142; - int sim_compid = 42; - uint8_t offboard_sysid = 134; - int offboard_compid = 1; - uint8_t vehicle_sysid = 135; - int vehicle_compid = 1; + loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); - // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi - // versus ethernet) then you will want to change the LocalHostIp accordingly. This default only works when log - // viewer and QGC are also on the same machine. Whatever network you choose it has to be the same one for - // external - std::string local_host_ip = "127.0.0.1"; + vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); + vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); - std::string model = "Generic"; + loadCameraSettings(settings_json, vehicle_setting->cameras, camera_defaults); + loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); - std::map params; - std::string logs; - }; + return vehicle_setting; + } - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + static void createDefaultVehicle(const std::string &simmode_name, + std::map> &vehicles, + const std::map> &sensor_defaults) { + vehicles.clear(); - struct SegmentationSetting { - enum class InitMethodType { None, CommonObjectsRandomIDs }; - - enum class MeshNamingMethodType { OwnerName, StaticMeshName }; - - InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; - bool override_existing = false; - MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; - }; - - struct TimeOfDaySetting { - bool enabled = false; - std::string start_datetime = ""; // format: %Y-%m-%d %H:%M:%S - bool is_start_datetime_dst = false; - float celestial_clock_speed = 1; - float update_interval_secs = 60; - bool move_sun = true; - }; - - private: // fields - float settings_version_actual; - float settings_version_minimum = 1.2f; - - public: // fields - std::string simmode_name = ""; - std::string level_name = ""; - - std::vector subwindow_settings; - RecordingSetting recording_setting; - SegmentationSetting segmentation_setting; - TimeOfDaySetting tod_setting; - - std::vector warning_messages; - std::vector error_messages; + // NOTE: Do not set defaults for vehicle type here. If you do then make sure + // to sync code in createVehicleSetting() as well. + if (simmode_name == kSimModeTypeMultirotor) { + // create simple flight as default multirotor + auto simple_flight_setting = + std::unique_ptr(new VehicleSetting("SimpleFlight", kVehicleTypeSimpleFlight)); + // TODO: we should be selecting remote if available else keyboard + // currently keyboard is not supported so use rc as default + simple_flight_setting->rc.remote_control_id = 0; + simple_flight_setting->sensors = sensor_defaults; + vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); + } else if (simmode_name == kSimModeTypeCar) { + // create PhysX as default car vehicle + auto physx_car_setting = + std::unique_ptr(new VehicleSetting("PhysXCar", kVehicleTypePhysXCar)); + physx_car_setting->sensors = sensor_defaults; + vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting); + } else if (simmode_name == kSimModeTypeComputerVision) { + // create default computer vision vehicle + auto cv_setting = + std::unique_ptr(new VehicleSetting("ComputerVision", kVehicleTypeComputerVision)); + cv_setting->sensors = sensor_defaults; + vehicles[cv_setting->vehicle_name] = std::move(cv_setting); + } else { + throw std::invalid_argument( + Utils::stringf("Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()) + .c_str()); + } + } - bool is_record_ui_visible = false; - int initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME - bool enable_rpc = true; - std::string api_server_address = ""; - int api_port = RpcLibPort; - std::string physics_engine_name = ""; + static void loadVehicleSettings(const std::string &simmode_name, const Settings &settings_json, + std::map> &vehicles, + std::map> &sensor_defaults, + const CameraSetting &camera_defaults) { + createDefaultVehicle(simmode_name, vehicles, sensor_defaults); - std::string clock_type = ""; - float clock_speed = 1.0f; - bool engine_sound = false; - bool log_messages_visible = true; - bool show_los_debug_lines_ = false; - HomeGeoPoint origin_geopoint{ - GeoPoint(47.641468, -122.140165, 122)}; // The geo-coordinate assigned to Unreal coordinate 0,0,0 - std::map pawn_paths; // path for pawn blueprint - std::map> vehicles; - CameraSetting camera_defaults; - CameraDirectorSetting camera_director; - float speed_unit_factor = 1.0f; - std::string speed_unit_label = "m\\s"; - std::map> sensor_defaults; - Vector3r wind = Vector3r::Zero(); - Vector3r ext_force = Vector3r::Zero(); - CameraSettingMap external_cameras; + nervosys::autonomylib::Settings vehicles_child; + if (settings_json.getChild("Vehicles", vehicles_child)) { + std::vector keys; + vehicles_child.getChildNames(keys); - std::string settings_text_ = ""; + // remove default vehicles, if values are specified in settings + if (keys.size()) + vehicles.clear(); - public: // methods - static AutonomySimSettings &singleton() { - static AutonomySimSettings instance; - return instance; + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + vehicles_child.getChild(key, child); + vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults, camera_defaults); + } + } } - AutonomySimSettings() { - initializeSubwindowSettings(subwindow_settings); - initializePawnPaths(pawn_paths); + static void initializePawnPaths(std::map &pawn_paths) { + pawn_paths.clear(); + pawn_paths.emplace("BareboneCar", + PawnPath("Class'/AutonomySim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); + pawn_paths.emplace("DefaultCar", PawnPath("Class'/AutonomySim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); + pawn_paths.emplace("DefaultQuadrotor", + PawnPath("Class'/AutonomySim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); + pawn_paths.emplace("DefaultComputerVision", + PawnPath("Class'/AutonomySim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'")); } - // returns number of warnings - void load(std::function simmode_getter) { - warning_messages.clear(); - error_messages.clear(); - const Settings &settings_json = Settings::singleton(); - checkSettingsVersion(settings_json); - - loadCoreSimModeSettings(settings_json, simmode_getter); - loadLevelSettings(settings_json); - loadDefaultCameraSetting(settings_json, camera_defaults); - loadCameraDirectorSetting(settings_json, camera_director, simmode_name); - loadSubWindowsSettings(settings_json, subwindow_settings); - loadViewModeSettings(settings_json); - loadSegmentationSetting(settings_json, segmentation_setting); - loadPawnPaths(settings_json, pawn_paths); - loadOtherSettings(settings_json); - loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); - loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults, camera_defaults); - loadExternalCameraSettings(settings_json, external_cameras, camera_defaults); + static void loadPawnPaths(const Settings &settings_json, std::map &pawn_paths) { + initializePawnPaths(pawn_paths); - // this should be done last because it depends on vehicles (and/or their type) we have - loadRecordingSetting(settings_json); - loadClockSettings(settings_json); - } + nervosys::autonomylib::Settings pawn_paths_child; + if (settings_json.getChild("PawnPaths", pawn_paths_child)) { + std::vector keys; + pawn_paths_child.getChildNames(keys); - static void initializeSettings(const std::string &json_settings_text) { - singleton().settings_text_ = json_settings_text; - Settings &settings_json = Settings::loadJSonString(json_settings_text); - if (!settings_json.isLoadSuccess()) - throw std::invalid_argument("Cannot parse JSON settings_json string."); + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + pawn_paths_child.getChild(key, child); + pawn_paths[key] = createPathPawn(child); + } + } } - static void createDefaultSettingsFile() { - initializeSettings("{}"); + static PawnPath createPathPawn(const Settings &settings_json) { + auto paths = PawnPath(); + paths.pawn_bp = settings_json.getString("PawnBP", ""); + auto slippery_mat = settings_json.getString("SlipperyMat", ""); + auto non_slippery_mat = settings_json.getString("NonSlipperyMat", ""); - Settings &settings_json = Settings::singleton(); - // write some settings_json in new file otherwise the string "null" is written if all settings_json are empty - settings_json.setString("SeeDocsAt", "https://github.com/nervosys/AutonomySim/blob/master/docs/settings.md"); - settings_json.setDouble("SettingsVersion", 1.2); + if (slippery_mat != "") + paths.slippery_mat = slippery_mat; + if (non_slippery_mat != "") + paths.non_slippery_mat = non_slippery_mat; - std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); - // TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only - // support Unreal 4.17 - // https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html - settings_json.saveJSonFile(settings_filename); + return paths; } - // This is for the case when a new vehicle is made on the fly, at runtime - void addVehicleSetting(const std::string &vehicle_name, const std::string &vehicle_type, const Pose &pose, - const std::string &pawn_path = "") { - // No Mavlink-type vehicles currently - auto vehicle_setting = std::unique_ptr(new VehicleSetting(vehicle_name, vehicle_type)); - vehicle_setting->position = pose.position; - vehicle_setting->pawn_path = pawn_path; - - vehicle_setting->sensors = sensor_defaults; + static void loadSegmentationSetting(const Settings &settings_json, SegmentationSetting &segmentation_setting) { + Settings json_parent; + if (settings_json.getChild("SegmentationSettings", json_parent)) { + std::string init_method = Utils::toLower(json_parent.getString("InitMethod", "")); + if (init_method == "" || init_method == "commonobjectsrandomids") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs; + else if (init_method == "none") + segmentation_setting.init_method = SegmentationSetting::InitMethodType::None; + else + // TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? + throw std::invalid_argument( + std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method); - VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, - vehicle_setting->rotation.yaw); + segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", true); - vehicles[vehicle_name] = std::move(vehicle_setting); + std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", "")); + if (mesh_naming_method == "" || mesh_naming_method == "ownername") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName; + else if (mesh_naming_method == "staticmeshname") + segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName; + else + throw std::invalid_argument( + std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + + mesh_naming_method); + } } - const VehicleSetting *getVehicleSetting(const std::string &vehicle_name) const { - auto it = vehicles.find(vehicle_name); - if (it == vehicles.end()) - // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight' - it = vehicles.find("SimpleFlight"); - return it->second.get(); + static void initializeNoiseSettings(NoiseSettingsMap &noise_settings) { + const int image_count = Utils::toNumeric(ImageType::Count); + noise_settings.clear(); + for (int i = -1; i < image_count; ++i) + noise_settings[i] = NoiseSetting(); } - static Vector3r createVectorSetting(const Settings &settings_json, const Vector3r &default_vec) { - return Vector3r(settings_json.getFloat("X", default_vec.x()), settings_json.getFloat("Y", default_vec.y()), - settings_json.getFloat("Z", default_vec.z())); - } - static Rotation createRotationSetting(const Settings &settings_json, const Rotation &default_rot) { - return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), - settings_json.getFloat("Pitch", default_rot.pitch), - settings_json.getFloat("Roll", default_rot.roll)); + static void loadNoiseSettings(const Settings &settings_json, NoiseSettingsMap &noise_settings) { + // We don't call initializeNoiseSettings here since it's already called in CameraSettings constructor + // And to avoid overwriting any defaults already set from CameraDefaults + + Settings json_parent; + if (settings_json.getChild("NoiseSettings", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + NoiseSetting noise_setting; + loadNoiseSetting(json_settings_child, noise_setting); + noise_settings[noise_setting.ImageType] = noise_setting; + } + } + } } - private: - void checkSettingsVersion(const Settings &settings_json) { - bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual); - bool upgrade_required = settings_version_actual < settings_version_minimum; - if (upgrade_required) { - bool auto_upgrade = false; + static void loadNoiseSetting(const Settings &settings_json, NoiseSetting &noise_setting) { + noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled); + noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType); - // if we have default setting file not modified by user then we will - // just auto-upgrade it - if (has_default_settings) { - auto_upgrade = true; - } else { - // check if auto-upgrade is possible - if (settings_version_actual == 1) { - const std::vector all_changed_keys = {"AdditionalCameras", "CaptureSettings", - "NoiseSettings", "UsageScenario", - "SimpleFlight", "PX4"}; - std::stringstream detected_keys_ss; - for (const auto &changed_key : all_changed_keys) { - if (settings_json.hasKey(changed_key)) - detected_keys_ss << changed_key << ","; - } - std::string detected_keys = detected_keys_ss.str(); - if (detected_keys.length()) { - std::string error_message = - "You are using newer version of AutonomySim with older version of settings.json. " - "You can either delete your settings.json and restart AutonomySim or use the upgrade " - "instructions at https://git.io/vjefh. \n\n" - "Following keys in your settings.json needs updating: "; + noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength); + noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed); + noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize); + noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity); + noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib); + noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib); + noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize); + noise_setting.HorzWaveScreenSize = + settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize); + noise_setting.HorzNoiseLinesContrib = + settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib); + noise_setting.HorzNoiseLinesDensityY = + settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY); + noise_setting.HorzNoiseLinesDensityXY = + settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY); + noise_setting.HorzDistortionStrength = + settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength); + noise_setting.HorzDistortionContrib = + settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); + } - error_messages.push_back(error_message + detected_keys); - } else - auto_upgrade = true; - } else - auto_upgrade = true; + static GimbalSetting createGimbalSetting(const Settings &settings_json) { + GimbalSetting gimbal; + // capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false); + gimbal.stabilization = settings_json.getFloat("Stabilization", false); + gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation); + return gimbal; + } + + static void loadUnrealEngineSetting(const nervosys::autonomylib::Settings &settings_json, + UnrealEngineSetting &ue_setting) { + Settings ue_settings_json; + if (settings_json.getChild("UnrealEngine", ue_settings_json)) { + Settings pixel_format_override_settings_json; + ue_setting.pixel_format_override_settings.clear(); + + for (int i = 0; i < Utils::toNumeric(ImageType::Count); i++) { + PixelFormatOverrideSetting pixel_format_setting; + pixel_format_setting.pixel_format = 0; // EXPixelformat::PF_Unknown + ue_setting.pixel_format_override_settings[i] = pixel_format_setting; } - if (auto_upgrade) { - warning_messages.push_back( - "You are using newer version of AutonomySim with older version of settings.json. " - "You should delete your settings.json file and restart AutonomySim."); + if (ue_settings_json.getChild("PixelFormatOverride", pixel_format_override_settings_json)) { + for (size_t child_index = 0; child_index < pixel_format_override_settings_json.size(); ++child_index) { + Settings pixel_format_child_json; + if (pixel_format_override_settings_json.getChild(child_index, pixel_format_child_json)) { + int image_type = pixel_format_child_json.getInt("ImageType", 0); + PixelFormatOverrideSetting &pixel_format_setting = + ue_setting.pixel_format_override_settings.at(image_type); + pixel_format_setting.pixel_format = + pixel_format_child_json.getInt("PixelFormat", 0); // default to EXPixelformat::PF_Unknown + } + } } } - // else no action necessary } - bool hasDefaultSettings(const Settings &settings_json, float &version) { - // if empty settings file - bool has_default = settings_json.size() == 0; + static CameraSetting createCameraSetting(const Settings &settings_json, const CameraSetting &camera_defaults) { + CameraSetting setting = camera_defaults; - bool has_docs = - settings_json.getString("SeeDocsAt", "") != "" || settings_json.getString("see_docs_at", "") != ""; - // we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :( - version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0)); + setting.position = createVectorSetting(settings_json, setting.position); + setting.rotation = createRotationSetting(settings_json, setting.rotation); - // If we have pre-V1 settings and only element is docs link - has_default |= settings_json.size() == 1 && has_docs; + loadCaptureSettings(settings_json, setting.capture_settings); + loadNoiseSettings(settings_json, setting.noise_settings); + Settings json_gimbal; + if (settings_json.getChild("Gimbal", json_gimbal)) + setting.gimbal = createGimbalSetting(json_gimbal); - // if we have V1 settings and only elements are docs link and version - has_default |= settings_json.size() == 2 && has_docs && version > 0; + loadUnrealEngineSetting(settings_json, setting.ue_setting); - return has_default; + return setting; } - void loadCoreSimModeSettings(const Settings &settings_json, std::function simmode_getter) { - // get the simmode from user if not specified - simmode_name = settings_json.getString("SimMode", ""); - if (simmode_name == "") { - if (simmode_getter) - simmode_name = simmode_getter(); - else - throw std::invalid_argument("simmode_name is not expected empty in SimModeBase"); - } + static void loadCameraSettings(const Settings &settings_json, CameraSettingMap &cameras, + const CameraSetting &camera_defaults) { + cameras.clear(); - physics_engine_name = settings_json.getString("PhysicsEngineName", ""); - if (physics_engine_name == "") { - if (simmode_name == kSimModeTypeMultirotor) - physics_engine_name = "FastPhysicsEngine"; - else - physics_engine_name = "PhysX"; // this value is only informational for now - } - } + Settings json_parent; + if (settings_json.getChild("Cameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); - void loadLevelSettings(const Settings &settings_json) { - level_name = settings_json.getString("Default Environment", ""); + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + json_parent.getChild(key, child); + cameras[key] = createCameraSetting(child, camera_defaults); + } + } } - void loadViewModeSettings(const Settings &settings_json) { - std::string view_mode_string = settings_json.getString("ViewMode", ""); - - if (view_mode_string == "") { - if (simmode_name == kSimModeTypeMultirotor) - view_mode_string = "FlyWithMe"; - else if (simmode_name == kSimModeTypeComputerVision) - view_mode_string = "Fpv"; - else - view_mode_string = "SpringArmChase"; - } + static void createCaptureSettings(const nervosys::autonomylib::Settings &settings_json, + CaptureSetting &capture_setting) { + capture_setting.width = settings_json.getInt("Width", capture_setting.width); + capture_setting.height = settings_json.getInt("Height", capture_setting.height); + capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees); + capture_setting.auto_exposure_speed = + settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed); + capture_setting.auto_exposure_bias = + settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias); + capture_setting.auto_exposure_max_brightness = + settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness); + capture_setting.auto_exposure_min_brightness = + settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness); + capture_setting.motion_blur_amount = + settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount); + capture_setting.image_type = settings_json.getInt("ImageType", 0); + capture_setting.target_gamma = settings_json.getFloat( + "TargetGamma", capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); - if (view_mode_string == "Fpv") - initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV; - else if (view_mode_string == "GroundObserver") - initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER; - else if (view_mode_string == "FlyWithMe") - initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME; - else if (view_mode_string == "Manual") - initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL; - else if (view_mode_string == "SpringArmChase") - initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE; - else if (view_mode_string == "Backup") - initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; - else if (view_mode_string == "NoDisplay") - initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; - else if (view_mode_string == "Front") - initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; + std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); + if (projection_mode == "" || projection_mode == "perspective") + capture_setting.projection_mode = 0; // Perspective + else if (projection_mode == "orthographic") + capture_setting.projection_mode = 1; // Orthographic else - error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); - } + throw std::invalid_argument( + std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode); - static void loadRCSetting(const std::string &simmode_name, const Settings &settings_json, RCSettings &rc_setting) { - Settings rc_json; - if (settings_json.getChild("RC", rc_json)) { - rc_setting.remote_control_id = - rc_json.getInt("RemoteControlID", simmode_name == kSimModeTypeMultirotor ? 0 : -1); - rc_setting.allow_api_when_disconnected = - rc_json.getBool("AllowAPIWhenDisconnected", rc_setting.allow_api_when_disconnected); - } + capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); } - static std::string getCameraName(const Settings &settings_json) { - return settings_json.getString( - "CameraName", - // TODO: below exist only due to legacy reason and can be replaced by "" in future - std::to_string(settings_json.getInt("CameraID", 0))); - } + static void loadSubWindowsSettings(const Settings &settings_json, + std::vector &subwindow_settings) { + // load default subwindows + initializeSubwindowSettings(subwindow_settings); - void loadDefaultRecordingSettings() { - recording_setting.requests.clear(); - // Add Scene image for each vehicle - for (const auto &vehicle : vehicles) { - recording_setting.requests[vehicle.first].push_back( - ImageCaptureBase::ImageRequest("", ImageType::Scene, false, true)); + Settings json_parent; + if (settings_json.getChild("SubWindows", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings json_settings_child; + if (json_parent.getChild(child_index, json_settings_child)) { + int window_index = json_settings_child.getInt("WindowID", 0); + SubwindowSetting &subwindow_setting = subwindow_settings.at(window_index); + subwindow_setting.window_index = window_index; + subwindow_setting.image_type = Utils::toEnum(json_settings_child.getInt("ImageType", 0)); + subwindow_setting.visible = json_settings_child.getBool("Visible", false); + subwindow_setting.camera_name = getCameraName(json_settings_child); + subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); + subwindow_setting.external = json_settings_child.getBool("External", false); + } + } } } - void loadRecordingSetting(const Settings &settings_json) { - loadDefaultRecordingSettings(); - - Settings recording_json; - if (settings_json.getChild("Recording", recording_json)) { - recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move); - recording_setting.record_interval = - recording_json.getFloat("RecordInterval", recording_setting.record_interval); - recording_setting.folder = recording_json.getString("Folder", recording_setting.folder); - recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled); - - Settings req_cameras_settings; - if (recording_json.getChild("Cameras", req_cameras_settings)) { - // If 'Cameras' field is present, clear defaults - recording_setting.requests.clear(); - // Get name of the default vehicle to be used if "VehicleName" isn't specified - // Map contains a default vehicle if vehicles haven't been specified - std::string default_vehicle_name = vehicles.begin()->first; + static void initializeSubwindowSettings(std::vector &subwindow_settings) { + subwindow_settings.clear(); + subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "", false)); // depth + subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "", false)); // seg + subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "", false)); // vis + } - for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { - Settings req_camera_settings; + void loadOtherSettings(const Settings &settings_json) { + // by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below + // because for docker container default is 0.0.0.0 and people get really confused why things + // don't work + api_server_address = settings_json.getString("LocalHostIp", ""); + api_port = settings_json.getInt("ApiServerPort", RpcLibPort); + is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); + engine_sound = settings_json.getBool("EngineSound", false); + enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); + speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f); + speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s"); + log_messages_visible = settings_json.getBool("LogMessagesVisible", true); + show_los_debug_lines_ = settings_json.getBool("ShowLosDebugLines", false); - if (req_cameras_settings.getChild(child_index, req_camera_settings)) { - std::string camera_name = getCameraName(req_camera_settings); - ImageType image_type = Utils::toEnum(req_camera_settings.getInt("ImageType", 0)); - bool compress = req_camera_settings.getBool("Compress", true); - bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false); - std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name); + { // load origin geopoint + Settings origin_geopoint_json; + if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) { + GeoPoint origin = origin_geopoint.home_geo_point; + origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude); + origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude); + origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); + origin_geopoint.initialize(origin); + } + } - recording_setting.requests[vehicle_name].push_back( - ImageCaptureBase::ImageRequest(camera_name, image_type, pixels_as_float, compress)); - } - } + { // time of day settings_json + Settings tod_settings_json; + if (settings_json.getChild("TimeOfDay", tod_settings_json)) { + tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled); + tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime); + tod_setting.celestial_clock_speed = + tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed); + tod_setting.is_start_datetime_dst = + tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst); + tod_setting.update_interval_secs = + tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs); + tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun); + } + } + + { + // Wind Settings + Settings child_json; + if (settings_json.getChild("Wind", child_json)) { + wind = createVectorSetting(child_json, wind); + } + } + { + // External Force Settings + Settings child_json; + if (settings_json.getChild("ExternalForce", child_json)) { + ext_force = createVectorSetting(child_json, ext_force); } } } - static void initializeCaptureSettings(CaptureSettingsMap &capture_settings) { - capture_settings.clear(); - for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) { - capture_settings[i] = CaptureSetting(); + static void loadDefaultCameraSetting(const Settings &settings_json, CameraSetting &camera_defaults) { + Settings child_json; + if (settings_json.getChild("CameraDefaults", child_json)) { + camera_defaults = createCameraSetting(child_json, camera_defaults); } - capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; } + static void loadCameraDirectorSetting(const Settings &settings_json, CameraDirectorSetting &camera_director, + const std::string &simmode_name) { + camera_director = CameraDirectorSetting(); - static void loadCaptureSettings(const Settings &settings_json, CaptureSettingsMap &capture_settings) { - // We don't call initializeCaptureSettings here since it's already called in CameraSettings constructor - // And to avoid overwriting any defaults already set from CameraDefaults + Settings child_json; + if (settings_json.getChild("CameraDirector", child_json)) { + camera_director.position = createVectorSetting(child_json, camera_director.position); + camera_director.rotation = createRotationSetting(child_json, camera_director.rotation); + camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance); + } - Settings json_parent; - if (settings_json.getChild("CaptureSettings", json_parent)) { - for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; - if (json_parent.getChild(child_index, json_settings_child)) { - CaptureSetting capture_setting; - createCaptureSettings(json_settings_child, capture_setting); - capture_settings[capture_setting.image_type] = capture_setting; + if (std::isnan(camera_director.follow_distance)) { + if (simmode_name == kSimModeTypeCar) + camera_director.follow_distance = -8; + else + camera_director.follow_distance = -3; + } + if (std::isnan(camera_director.position.x())) + camera_director.position.x() = camera_director.follow_distance; + if (std::isnan(camera_director.position.y())) + camera_director.position.y() = 0; + if (std::isnan(camera_director.position.z())) { + if (simmode_name == kSimModeTypeCar) + camera_director.position.z() = -4; + else + camera_director.position.z() = -2; + } + } + + void loadClockSettings(const Settings &settings_json) { + clock_type = settings_json.getString("ClockType", ""); + + if (clock_type == "") { + // default value + clock_type = "ScalableClock"; + + // override if multirotor simmode with simple_flight + if (simmode_name == kSimModeTypeMultirotor) { + // TODO: this won't work if simple_flight and PX4 is combined together! + + // for multirotors we select steppable fixed interval clock unless we have + // PX4 enabled vehicle + clock_type = "SteppableClock"; + for (auto const &vehicle : vehicles) { + if (vehicle.second->auto_create && vehicle.second->vehicle_type == kVehicleTypePX4) { + clock_type = "ScalableClock"; + break; + } } } } + + clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); } - static std::unique_ptr createMavLinkVehicleSetting(const Settings &settings_json) { - // these settings_json are expected in same section, not in another child - std::unique_ptr vehicle_setting_p = - std::unique_ptr(new MavLinkVehicleSetting()); - MavLinkVehicleSetting *vehicle_setting = static_cast(vehicle_setting_p.get()); + static std::shared_ptr createSensorSetting(SensorBase::SensorType sensor_type, + const std::string &sensor_name, bool enabled) { + std::shared_ptr sensor_setting; - // TODO: we should be selecting remote if available else keyboard - // currently keyboard is not supported so use rc as default - vehicle_setting->rc.remote_control_id = 0; + switch (sensor_type) { + case SensorBase::SensorType::Barometer: + sensor_setting = std::shared_ptr(new BarometerSetting()); + break; + case SensorBase::SensorType::Imu: + sensor_setting = std::shared_ptr(new ImuSetting()); + break; + case SensorBase::SensorType::Gps: + sensor_setting = std::shared_ptr(new GpsSetting()); + break; + case SensorBase::SensorType::Magnetometer: + sensor_setting = std::shared_ptr(new MagnetometerSetting()); + break; + case SensorBase::SensorType::Distance: + sensor_setting = std::shared_ptr(new DistanceSetting()); + break; + case SensorBase::SensorType::Lidar: + sensor_setting = std::shared_ptr(new LidarSetting()); + break; + default: + throw std::invalid_argument("Unexpected sensor type"); + } - MavLinkConnectionInfo &connection_info = vehicle_setting->connection_info; - connection_info.sim_sysid = static_cast(settings_json.getInt("SimSysID", connection_info.sim_sysid)); - connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid); + sensor_setting->sensor_type = sensor_type; + sensor_setting->sensor_name = sensor_name; + sensor_setting->enabled = enabled; - connection_info.vehicle_sysid = - static_cast(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid)); - connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid); + return sensor_setting; + } - connection_info.offboard_sysid = - static_cast(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid)); - connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid); + static void initializeSensorSetting(SensorSetting *sensor_setting, const Settings &settings_json) { + sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); - connection_info.logviewer_ip_address = - settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address); - connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port); - connection_info.logviewer_ip_sport = - settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport); + // pass the json Settings property bag through to the specific sensor params object where it is + // extracted there. This way default values can be kept in one place. For example, see the + // BarometerSimpleParams::initializeFromSettings method. + sensor_setting->settings = settings_json; + } - connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address); - connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port); + // creates and intializes sensor settings from json + static void loadSensorSettings(const Settings &settings_json, const std::string &collectionName, + std::map> &sensors, + std::map> &sensor_defaults) - connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address); - connection_info.control_port_local = - settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy - connection_info.control_port_local = - settings_json.getInt("ControlPortLocal", connection_info.control_port_local); - connection_info.control_port_remote = - settings_json.getInt("ControlPortRemote", connection_info.control_port_remote); + { + // NOTE: Increase type if number of sensors goes above 8 + uint8_t present_sensors_bitmask = 0; - std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address); - if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) { - // backwards compat - connection_info.control_ip_address = sitlip; - } - if (settings_json.hasKey("SitlPort")) { - // backwards compat - connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local); - } + nervosys::autonomylib::Settings sensors_child; + if (settings_json.getChild(collectionName, sensors_child)) { + std::vector keys; + sensors_child.getChildNames(keys); - connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + sensors_child.getChild(key, child); - connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial); - connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address); - connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port); - connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp); - connection_info.lock_step = settings_json.getBool("LockStep", connection_info.lock_step); - connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port); - connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port); - connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate); - connection_info.model = settings_json.getString("Model", connection_info.model); - connection_info.logs = settings_json.getString("Logs", connection_info.logs); + auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); + auto enabled = child.getBool("Enabled", false); - Settings params; - if (settings_json.getChild("Parameters", params)) { - std::vector keys; - params.getChildNames(keys); - for (auto key : keys) { - connection_info.params[key] = params.getFloat(key, 0); + sensors[key] = createSensorSetting(sensor_type, key, enabled); + initializeSensorSetting(sensors[key].get(), child); + + // Mark sensor types already added + present_sensors_bitmask |= 1U << Utils::toNumeric(sensor_type); } } - return vehicle_setting_p; + // Only add default sensors which are not present + for (const auto &p : sensor_defaults) { + auto type = Utils::toNumeric(p.second->sensor_type); + + if ((present_sensors_bitmask & (1U << type)) == 0) + sensors[p.first] = p.second; + } } - static std::unique_ptr - createVehicleSetting(const std::string &simmode_name, const Settings &settings_json, const std::string vehicle_name, - std::map> &sensor_defaults, - const CameraSetting &camera_defaults) { - auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", "")); - - std::unique_ptr vehicle_setting; - if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || - vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover) - vehicle_setting = createMavLinkVehicleSetting(settings_json); - // for everything else we don't need derived class yet - else { - vehicle_setting = std::unique_ptr(new VehicleSetting()); - if (vehicle_type == kVehicleTypeSimpleFlight) { - // TODO: we should be selecting remote if available else keyboard - // currently keyboard is not supported so use rc as default - vehicle_setting->rc.remote_control_id = 0; - } - } - vehicle_setting->vehicle_name = vehicle_name; - - // required settings_json - vehicle_setting->vehicle_type = vehicle_type; - - // optional settings_json - vehicle_setting->pawn_path = settings_json.getString("PawnPath", ""); - vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", ""); - vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways", vehicle_setting->allow_api_always); - vehicle_setting->auto_create = settings_json.getBool("AutoCreate", vehicle_setting->auto_create); - vehicle_setting->enable_collision_passthrough = - settings_json.getBool("EnableCollisionPassthrogh", vehicle_setting->enable_collision_passthrough); - vehicle_setting->enable_trace = settings_json.getBool("EnableTrace", vehicle_setting->enable_trace); - vehicle_setting->enable_collisions = - settings_json.getBool("EnableCollisions", vehicle_setting->enable_collisions); - vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle", vehicle_setting->is_fpv_vehicle); - - loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); - - vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); - vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); - - loadCameraSettings(settings_json, vehicle_setting->cameras, camera_defaults); - loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); - - return vehicle_setting; - } - - static void createDefaultVehicle(const std::string &simmode_name, - std::map> &vehicles, - const std::map> &sensor_defaults) { - vehicles.clear(); - - // NOTE: Do not set defaults for vehicle type here. If you do then make sure - // to sync code in createVehicleSetting() as well. + // creates default sensor list when none specified in json + static void createDefaultSensorSettings(const std::string &simmode_name, + std::map> &sensors) { if (simmode_name == kSimModeTypeMultirotor) { - // create simple flight as default multirotor - auto simple_flight_setting = - std::unique_ptr(new VehicleSetting("SimpleFlight", kVehicleTypeSimpleFlight)); - // TODO: we should be selecting remote if available else keyboard - // currently keyboard is not supported so use rc as default - simple_flight_setting->rc.remote_control_id = 0; - simple_flight_setting->sensors = sensor_defaults; - vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); + sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); + sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); + sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true); } else if (simmode_name == kSimModeTypeCar) { - // create PhysX as default car vehicle - auto physx_car_setting = - std::unique_ptr(new VehicleSetting("PhysXCar", kVehicleTypePhysXCar)); - physx_car_setting->sensors = sensor_defaults; - vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting); - } else if (simmode_name == kSimModeTypeComputerVision) { - // create default computer vision vehicle - auto cv_setting = - std::unique_ptr(new VehicleSetting("ComputerVision", kVehicleTypeComputerVision)); - cv_setting->sensors = sensor_defaults; - vehicles[cv_setting->vehicle_name] = std::move(cv_setting); + sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); } else { - throw std::invalid_argument( - Utils::stringf("Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()) - .c_str()); + // no sensors added for other modes } } - static void loadVehicleSettings(const std::string &simmode_name, const Settings &settings_json, - std::map> &vehicles, - std::map> &sensor_defaults, - const CameraSetting &camera_defaults) { - createDefaultVehicle(simmode_name, vehicles, sensor_defaults); + // loads or creates default sensor list + static void loadDefaultSensorSettings(const std::string &simmode_name, const Settings &settings_json, + std::map> &sensors) { + nervosys::autonomylib::Settings sensors_child; + if (settings_json.getChild("DefaultSensors", sensors_child)) + loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors); + else + createDefaultSensorSettings(simmode_name, sensors); + } - nervosys::autonomylib::Settings vehicles_child; - if (settings_json.getChild("Vehicles", vehicles_child)) { - std::vector keys; - vehicles_child.getChildNames(keys); + static void loadExternalCameraSettings(const Settings &settings_json, CameraSettingMap &external_cameras, + const CameraSetting &camera_defaults) { + external_cameras.clear(); - // remove default vehicles, if values are specified in settings - if (keys.size()) - vehicles.clear(); + Settings json_parent; + if (settings_json.getChild("ExternalCameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - vehicles_child.getChild(key, child); - vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults, camera_defaults); + Settings child; + json_parent.getChild(key, child); + external_cameras[key] = createCameraSetting(child, camera_defaults); } } } - static void initializePawnPaths(std::map &pawn_paths) { - pawn_paths.clear(); - pawn_paths.emplace("BareboneCar", - PawnPath("Class'/AutonomySim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); - pawn_paths.emplace("DefaultCar", PawnPath("Class'/AutonomySim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); - pawn_paths.emplace("DefaultQuadrotor", - PawnPath("Class'/AutonomySim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); - pawn_paths.emplace("DefaultComputerVision", - PawnPath("Class'/AutonomySim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'")); - } + public: + // types + static constexpr int kSubwindowCount = 3; // must be >= 3 for now + static constexpr char const *kVehicleTypePX4 = "px4multirotor"; + static constexpr char const *kVehicleTypeArduCopterSolo = "arducoptersolo"; + static constexpr char const *kVehicleTypeSimpleFlight = "simpleflight"; + static constexpr char const *kVehicleTypeArduCopter = "arducopter"; + static constexpr char const *kVehicleTypePhysXCar = "physxcar"; + static constexpr char const *kVehicleTypeArduRover = "ardurover"; + static constexpr char const *kVehicleTypeComputerVision = "computervision"; - static void loadPawnPaths(const Settings &settings_json, std::map &pawn_paths) { - initializePawnPaths(pawn_paths); + static constexpr char const *kVehicleInertialFrame = "VehicleInertialFrame"; + static constexpr char const *kSensorLocalFrame = "SensorLocalFrame"; - nervosys::autonomylib::Settings pawn_paths_child; - if (settings_json.getChild("PawnPaths", pawn_paths_child)) { - std::vector keys; - pawn_paths_child.getChildNames(keys); + static constexpr char const *kSimModeTypeMultirotor = "Multirotor"; + static constexpr char const *kSimModeTypeCar = "Car"; + static constexpr char const *kSimModeTypeComputerVision = "ComputerVision"; - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - pawn_paths_child.getChild(key, child); - pawn_paths[key] = createPathPawn(child); - } - } - } + struct SubwindowSetting { + int window_index; + ImageType image_type; + bool visible; + std::string camera_name; + std::string vehicle_name; + bool external; - static PawnPath createPathPawn(const Settings &settings_json) { - auto paths = PawnPath(); - paths.pawn_bp = settings_json.getString("PawnBP", ""); - auto slippery_mat = settings_json.getString("SlipperyMat", ""); - auto non_slippery_mat = settings_json.getString("NonSlipperyMat", ""); + SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, + bool visible_val = false, const std::string &camera_name_val = "", + const std::string &vehicle_name_val = "", bool external_val = false) + : window_index(window_index_val), image_type(image_type_val), visible(visible_val), + camera_name(camera_name_val), vehicle_name(vehicle_name_val), external(external_val) {} + }; - if (slippery_mat != "") - paths.slippery_mat = slippery_mat; - if (non_slippery_mat != "") - paths.non_slippery_mat = non_slippery_mat; + struct RecordingSetting { + bool record_on_move = false; + float record_interval = 0.05f; + std::string folder = ""; + bool enabled = false; - return paths; - } + std::map> requests; - static void loadSegmentationSetting(const Settings &settings_json, SegmentationSetting &segmentation_setting) { - Settings json_parent; - if (settings_json.getChild("SegmentationSettings", json_parent)) { - std::string init_method = Utils::toLower(json_parent.getString("InitMethod", "")); - if (init_method == "" || init_method == "commonobjectsrandomids") - segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs; - else if (init_method == "none") - segmentation_setting.init_method = SegmentationSetting::InitMethodType::None; - else - // TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? - throw std::invalid_argument( - std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method); + RecordingSetting() {} - segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", true); + RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string &folder_val, + bool enabled_val) + : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), + enabled(enabled_val) {} + }; - std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", "")); - if (mesh_naming_method == "" || mesh_naming_method == "ownername") - segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName; - else if (mesh_naming_method == "staticmeshname") - segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName; - else - throw std::invalid_argument( - std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + - mesh_naming_method); - } - } + struct PawnPath { + std::string pawn_bp; + std::string slippery_mat; + std::string non_slippery_mat; - static void initializeNoiseSettings(NoiseSettingsMap &noise_settings) { - const int image_count = Utils::toNumeric(ImageType::Count); - noise_settings.clear(); - for (int i = -1; i < image_count; ++i) - noise_settings[i] = NoiseSetting(); - } + PawnPath(const std::string &pawn_bp_val = "", + const std::string &slippery_mat_val = "/AutonomySim/VehicleAdv/PhysicsMaterials/Slippery.Slippery", + const std::string &non_slippery_mat_val = + "/AutonomySim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") + : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) {} + }; - static void loadNoiseSettings(const Settings &settings_json, NoiseSettingsMap &noise_settings) { - // We don't call initializeNoiseSettings here since it's already called in CameraSettings constructor - // And to avoid overwriting any defaults already set from CameraDefaults + struct RCSettings { + int remote_control_id = -1; + bool allow_api_when_disconnected = false; + }; - Settings json_parent; - if (settings_json.getChild("NoiseSettings", json_parent)) { - for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; - if (json_parent.getChild(child_index, json_settings_child)) { - NoiseSetting noise_setting; - loadNoiseSetting(json_settings_child, noise_setting); - noise_settings[noise_setting.ImageType] = noise_setting; - } - } - } - } + struct Rotation { + float yaw = 0; + float pitch = 0; + float roll = 0; - static void loadNoiseSetting(const Settings &settings_json, NoiseSetting &noise_setting) { - noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled); - noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType); + Rotation() {} - noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength); - noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed); - noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize); - noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity); - noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib); - noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib); - noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize); - noise_setting.HorzWaveScreenSize = - settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize); - noise_setting.HorzNoiseLinesContrib = - settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib); - noise_setting.HorzNoiseLinesDensityY = - settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY); - noise_setting.HorzNoiseLinesDensityXY = - settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY); - noise_setting.HorzDistortionStrength = - settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength); - noise_setting.HorzDistortionContrib = - settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); - } + Rotation(float yaw_val, float pitch_val, float roll_val) : yaw(yaw_val), pitch(pitch_val), roll(roll_val) {} - static GimbalSetting createGimbalSetting(const Settings &settings_json) { - GimbalSetting gimbal; - // capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false); - gimbal.stabilization = settings_json.getFloat("Stabilization", false); - gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation); - return gimbal; - } + static Rotation nanRotation() noexcept { + static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); + return val; + } + }; - static void loadUnrealEngineSetting(const nervosys::autonomylib::Settings &settings_json, - UnrealEngineSetting &ue_setting) { - Settings ue_settings_json; - if (settings_json.getChild("UnrealEngine", ue_settings_json)) { - Settings pixel_format_override_settings_json; - ue_setting.pixel_format_override_settings.clear(); + struct GimbalSetting { + float stabilization = 0; + // bool is_world_frame = false; + Rotation rotation = Rotation::nanRotation(); + }; - for (int i = 0; i < Utils::toNumeric(ImageType::Count); i++) { - PixelFormatOverrideSetting pixel_format_setting; - pixel_format_setting.pixel_format = 0; // EXPixelformat::PF_Unknown - ue_setting.pixel_format_override_settings[i] = pixel_format_setting; - } + struct CaptureSetting { + // below settings_json are obtained by using Unreal console command (press ~): + // ShowFlag.VisualizeHDR 1. + // to replicate camera settings_json to SceneCapture2D + // TODO: should we use UAutonomyBlueprintLib::GetDisplayGamma()? + static constexpr float kSceneTargetGamma = 1.4f; - if (ue_settings_json.getChild("PixelFormatOverride", pixel_format_override_settings_json)) { - for (size_t child_index = 0; child_index < pixel_format_override_settings_json.size(); ++child_index) { - Settings pixel_format_child_json; - if (pixel_format_override_settings_json.getChild(child_index, pixel_format_child_json)) { - int image_type = pixel_format_child_json.getInt("ImageType", 0); - PixelFormatOverrideSetting &pixel_format_setting = - ue_setting.pixel_format_override_settings.at(image_type); - pixel_format_setting.pixel_format = - pixel_format_child_json.getInt("PixelFormat", 0); // default to EXPixelformat::PF_Unknown - } - } - } - } - } + int image_type = 0; - static CameraSetting createCameraSetting(const Settings &settings_json, const CameraSetting &camera_defaults) { - CameraSetting setting = camera_defaults; + unsigned int width = 256, height = 144; // 960 X 540 + float fov_degrees = Utils::nan(); // 90.0f + int auto_exposure_method = -1; // histogram + float auto_exposure_speed = Utils::nan(); // 100.0f; + float auto_exposure_bias = Utils::nan(); // 0; + float auto_exposure_max_brightness = Utils::nan(); // 0.64f; + float auto_exposure_min_brightness = Utils::nan(); // 0.03f; + float auto_exposure_low_percent = Utils::nan(); // 80.0f; + float auto_exposure_high_percent = Utils::nan(); // 98.3f; + float auto_exposure_histogram_log_min = Utils::nan(); // -8; + float auto_exposure_histogram_log_max = Utils::nan(); // 4; + float motion_blur_amount = Utils::nan(); + float target_gamma = + Utils::nan(); // 1.0f; //This would be reset to kSceneTargetGamma for scene as default + int projection_mode = 0; // ECameraProjectionMode::Perspective + float ortho_width = Utils::nan(); + }; - setting.position = createVectorSetting(settings_json, setting.position); - setting.rotation = createRotationSetting(settings_json, setting.rotation); + struct NoiseSetting { + int ImageType = 0; - loadCaptureSettings(settings_json, setting.capture_settings); - loadNoiseSettings(settings_json, setting.noise_settings); - Settings json_gimbal; - if (settings_json.getChild("Gimbal", json_gimbal)) - setting.gimbal = createGimbalSetting(json_gimbal); + bool Enabled = false; - loadUnrealEngineSetting(settings_json, setting.ue_setting); + float RandContrib = 0.2f; + float RandSpeed = 100000.0f; + float RandSize = 500.0f; + float RandDensity = 2.0f; - return setting; - } + float HorzWaveContrib = 0.03f; + float HorzWaveStrength = 0.08f; + float HorzWaveVertSize = 1.0f; + float HorzWaveScreenSize = 1.0f; - static void loadCameraSettings(const Settings &settings_json, CameraSettingMap &cameras, - const CameraSetting &camera_defaults) { - cameras.clear(); + float HorzNoiseLinesContrib = 1.0f; + float HorzNoiseLinesDensityY = 0.01f; + float HorzNoiseLinesDensityXY = 0.5f; - Settings json_parent; - if (settings_json.getChild("Cameras", json_parent)) { - std::vector keys; - json_parent.getChildNames(keys); + float HorzDistortionContrib = 1.0f; + float HorzDistortionStrength = 0.002f; + }; - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - json_parent.getChild(key, child); - cameras[key] = createCameraSetting(child, camera_defaults); - } - } - } + struct PixelFormatOverrideSetting { + int pixel_format = 0; + }; - static void createCaptureSettings(const nervosys::autonomylib::Settings &settings_json, - CaptureSetting &capture_setting) { - capture_setting.width = settings_json.getInt("Width", capture_setting.width); - capture_setting.height = settings_json.getInt("Height", capture_setting.height); - capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees); - capture_setting.auto_exposure_speed = - settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed); - capture_setting.auto_exposure_bias = - settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias); - capture_setting.auto_exposure_max_brightness = - settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness); - capture_setting.auto_exposure_min_brightness = - settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness); - capture_setting.motion_blur_amount = - settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount); - capture_setting.image_type = settings_json.getInt("ImageType", 0); - capture_setting.target_gamma = settings_json.getFloat( - "TargetGamma", capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan()); + struct UnrealEngineSetting { + std::map pixel_format_override_settings; + }; - std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", "")); - if (projection_mode == "" || projection_mode == "perspective") - capture_setting.projection_mode = 0; // Perspective - else if (projection_mode == "orthographic") - capture_setting.projection_mode = 1; // Orthographic - else - throw std::invalid_argument( - std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode); + using CaptureSettingsMap = std::map; + using NoiseSettingsMap = std::map; + struct CameraSetting { + // nan means keep the default values set in components + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); - capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); - } + GimbalSetting gimbal; + CaptureSettingsMap capture_settings; + NoiseSettingsMap noise_settings; - static void loadSubWindowsSettings(const Settings &settings_json, - std::vector &subwindow_settings) { - // load default subwindows - initializeSubwindowSettings(subwindow_settings); + UnrealEngineSetting ue_setting; - Settings json_parent; - if (settings_json.getChild("SubWindows", json_parent)) { - for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { - Settings json_settings_child; - if (json_parent.getChild(child_index, json_settings_child)) { - int window_index = json_settings_child.getInt("WindowID", 0); - SubwindowSetting &subwindow_setting = subwindow_settings.at(window_index); - subwindow_setting.window_index = window_index; - subwindow_setting.image_type = Utils::toEnum(json_settings_child.getInt("ImageType", 0)); - subwindow_setting.visible = json_settings_child.getBool("Visible", false); - subwindow_setting.camera_name = getCameraName(json_settings_child); - subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", ""); - subwindow_setting.external = json_settings_child.getBool("External", false); - } - } + CameraSetting() { + initializeCaptureSettings(capture_settings); + initializeNoiseSettings(noise_settings); } - } + }; + using CameraSettingMap = std::map; - static void initializeSubwindowSettings(std::vector &subwindow_settings) { - subwindow_settings.clear(); - subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "", false)); // depth - subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "", false)); // seg - subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "", false)); // vis - } + struct CameraDirectorSetting { + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + float follow_distance = Utils::nan(); + }; - void loadOtherSettings(const Settings &settings_json) { - // by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below - // because for docker container default is 0.0.0.0 and people get really confused why things - // don't work - api_server_address = settings_json.getString("LocalHostIp", ""); - api_port = settings_json.getInt("ApiServerPort", RpcLibPort); - is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); - engine_sound = settings_json.getBool("EngineSound", false); - enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); - speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f); - speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s"); - log_messages_visible = settings_json.getBool("LogMessagesVisible", true); - show_los_debug_lines_ = settings_json.getBool("ShowLosDebugLines", false); + struct SensorSetting { + SensorBase::SensorType sensor_type; + std::string sensor_name; + bool enabled = true; + Settings settings; // imported json data that needs to be parsed by specific sensors. + }; - { // load origin geopoint - Settings origin_geopoint_json; - if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) { - GeoPoint origin = origin_geopoint.home_geo_point; - origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude); - origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude); - origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); - origin_geopoint.initialize(origin); - } - } + struct BarometerSetting : SensorSetting {}; - { // time of day settings_json - Settings tod_settings_json; - if (settings_json.getChild("TimeOfDay", tod_settings_json)) { - tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled); - tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime); - tod_setting.celestial_clock_speed = - tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed); - tod_setting.is_start_datetime_dst = - tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst); - tod_setting.update_interval_secs = - tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs); - tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun); - } - } + struct ImuSetting : SensorSetting {}; - { - // Wind Settings - Settings child_json; - if (settings_json.getChild("Wind", child_json)) { - wind = createVectorSetting(child_json, wind); - } - } - { - // External Force Settings - Settings child_json; - if (settings_json.getChild("ExternalForce", child_json)) { - ext_force = createVectorSetting(child_json, ext_force); - } - } - } + struct GpsSetting : SensorSetting {}; - static void loadDefaultCameraSetting(const Settings &settings_json, CameraSetting &camera_defaults) { - Settings child_json; - if (settings_json.getChild("CameraDefaults", child_json)) { - camera_defaults = createCameraSetting(child_json, camera_defaults); - } - } - static void loadCameraDirectorSetting(const Settings &settings_json, CameraDirectorSetting &camera_director, - const std::string &simmode_name) { - camera_director = CameraDirectorSetting(); + struct MagnetometerSetting : SensorSetting {}; - Settings child_json; - if (settings_json.getChild("CameraDirector", child_json)) { - camera_director.position = createVectorSetting(child_json, camera_director.position); - camera_director.rotation = createRotationSetting(child_json, camera_director.rotation); - camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance); - } + struct DistanceSetting : SensorSetting {}; - if (std::isnan(camera_director.follow_distance)) { - if (simmode_name == kSimModeTypeCar) - camera_director.follow_distance = -8; - else - camera_director.follow_distance = -3; - } - if (std::isnan(camera_director.position.x())) - camera_director.position.x() = camera_director.follow_distance; - if (std::isnan(camera_director.position.y())) - camera_director.position.y() = 0; - if (std::isnan(camera_director.position.z())) { - if (simmode_name == kSimModeTypeCar) - camera_director.position.z() = -4; - else - camera_director.position.z() = -2; - } - } + struct LidarSetting : SensorSetting { + enum class DataFrame { VehicleInertialFrame, SensorLocalFrame }; + }; - void loadClockSettings(const Settings &settings_json) { - clock_type = settings_json.getString("ClockType", ""); + struct VehicleSetting { + // required + std::string vehicle_name; + std::string vehicle_type; + + // optional + std::string default_vehicle_state; + std::string pawn_path; + bool allow_api_always = true; + bool auto_create = true; + bool enable_collision_passthrough = false; + bool enable_trace = false; + bool enable_collisions = true; + bool is_fpv_vehicle = false; + + // nan means use player start + Vector3r position = VectorMath::nanVector(); // in global NED + Rotation rotation = Rotation::nanRotation(); + + CameraSettingMap cameras; + std::map> sensors; + + RCSettings rc; + + VehicleSetting() {} + + VehicleSetting(const std::string &vehicle_name_val, const std::string &vehicle_type_val) + : vehicle_name(vehicle_name_val), vehicle_type(vehicle_type_val) {} + }; + + struct MavLinkConnectionInfo { + /* Default values are requires so uninitialized instance doesn't have random values */ + + bool use_serial = true; // false means use UDP or TCP instead + + // Used to connect via HITL: needed only if use_serial = true + std::string serial_port = "*"; + int baud_rate = 115200; + + // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false + std::string udp_address = "127.0.0.1"; + int udp_port = 14560; + + // Used to accept connections from drone over TCP: needed only if use_tcp = true + bool lock_step = true; + bool use_tcp = false; + int tcp_port = 4560; + + // The PX4 SITL app requires receiving drone commands over a different mavlink channel called + // the "ground control station" (or GCS) channel. + std::string control_ip_address = "127.0.0.1"; + int control_port_local = 14540; + int control_port_remote = 14580; + + // The log viewer can be on a different machine, so you can configure it's ip address and port here. + int logviewer_ip_port = 14388; + int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle. + std::string logviewer_ip_address = ""; + + // The QGroundControl app can be on a different machine, and AutonomySim can act as a proxy to forward + // the mavlink stream over to that machine if you configure it's ip address and port here. + int qgc_ip_port = 0; + std::string qgc_ip_address = ""; + + // mavlink vehicle identifiers + uint8_t sim_sysid = 142; + int sim_compid = 42; + uint8_t offboard_sysid = 134; + int offboard_compid = 1; + uint8_t vehicle_sysid = 135; + int vehicle_compid = 1; - if (clock_type == "") { - // default value - clock_type = "ScalableClock"; + // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi + // versus ethernet) then you will want to change the LocalHostIp accordingly. This default only works when log + // viewer and QGC are also on the same machine. Whatever network you choose it has to be the same one for + // external + std::string local_host_ip = "127.0.0.1"; - // override if multirotor simmode with simple_flight - if (simmode_name == kSimModeTypeMultirotor) { - // TODO: this won't work if simple_flight and PX4 is combined together! + std::string model = "Generic"; - // for multirotors we select steppable fixed interval clock unless we have - // PX4 enabled vehicle - clock_type = "SteppableClock"; - for (auto const &vehicle : vehicles) { - if (vehicle.second->auto_create && vehicle.second->vehicle_type == kVehicleTypePX4) { - clock_type = "ScalableClock"; - break; - } - } - } - } + std::map params; + std::string logs; + }; - clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); - } + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; - static std::shared_ptr createSensorSetting(SensorBase::SensorType sensor_type, - const std::string &sensor_name, bool enabled) { - std::shared_ptr sensor_setting; + struct SegmentationSetting { + enum class InitMethodType { None, CommonObjectsRandomIDs }; - switch (sensor_type) { - case SensorBase::SensorType::Barometer: - sensor_setting = std::shared_ptr(new BarometerSetting()); - break; - case SensorBase::SensorType::Imu: - sensor_setting = std::shared_ptr(new ImuSetting()); - break; - case SensorBase::SensorType::Gps: - sensor_setting = std::shared_ptr(new GpsSetting()); - break; - case SensorBase::SensorType::Magnetometer: - sensor_setting = std::shared_ptr(new MagnetometerSetting()); - break; - case SensorBase::SensorType::Distance: - sensor_setting = std::shared_ptr(new DistanceSetting()); - break; - case SensorBase::SensorType::Lidar: - sensor_setting = std::shared_ptr(new LidarSetting()); - break; - default: - throw std::invalid_argument("Unexpected sensor type"); - } + enum class MeshNamingMethodType { OwnerName, StaticMeshName }; - sensor_setting->sensor_type = sensor_type; - sensor_setting->sensor_name = sensor_name; - sensor_setting->enabled = enabled; + InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; + bool override_existing = false; + MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; + }; - return sensor_setting; - } + struct TimeOfDaySetting { + bool enabled = false; + std::string start_datetime = ""; // format: %Y-%m-%d %H:%M:%S + bool is_start_datetime_dst = false; + float celestial_clock_speed = 1; + float update_interval_secs = 60; + bool move_sun = true; + }; - static void initializeSensorSetting(SensorSetting *sensor_setting, const Settings &settings_json) { - sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); + // fields + std::string simmode_name = ""; + std::string level_name = ""; - // pass the json Settings property bag through to the specific sensor params object where it is - // extracted there. This way default values can be kept in one place. For example, see the - // BarometerSimpleParams::initializeFromSettings method. - sensor_setting->settings = settings_json; - } + std::vector subwindow_settings; + RecordingSetting recording_setting; + SegmentationSetting segmentation_setting; + TimeOfDaySetting tod_setting; - // creates and intializes sensor settings from json - static void loadSensorSettings(const Settings &settings_json, const std::string &collectionName, - std::map> &sensors, - std::map> &sensor_defaults) + std::vector warning_messages; + std::vector error_messages; - { - // NOTE: Increase type if number of sensors goes above 8 - uint8_t present_sensors_bitmask = 0; + bool is_record_ui_visible = false; + int initial_view_mode = 2; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME + bool enable_rpc = true; + std::string api_server_address = ""; + int api_port = RpcLibPort; + std::string physics_engine_name = ""; - nervosys::autonomylib::Settings sensors_child; - if (settings_json.getChild(collectionName, sensors_child)) { - std::vector keys; - sensors_child.getChildNames(keys); + std::string clock_type = ""; + float clock_speed = 1.0f; + bool engine_sound = false; + bool log_messages_visible = true; + bool show_los_debug_lines_ = false; + HomeGeoPoint origin_geopoint{ + GeoPoint(47.641468, -122.140165, 122)}; // The geo-coordinate assigned to Unreal coordinate 0,0,0 + std::map pawn_paths; // path for pawn blueprint + std::map> vehicles; + CameraSetting camera_defaults; + CameraDirectorSetting camera_director; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; + std::map> sensor_defaults; + Vector3r wind = Vector3r::Zero(); + Vector3r ext_force = Vector3r::Zero(); + CameraSettingMap external_cameras; - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - sensors_child.getChild(key, child); + std::string settings_text_ = ""; - auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); - auto enabled = child.getBool("Enabled", false); + // methods + static AutonomySimSettings &singleton() { + static AutonomySimSettings instance; + return instance; + } - sensors[key] = createSensorSetting(sensor_type, key, enabled); - initializeSensorSetting(sensors[key].get(), child); + AutonomySimSettings() { + initializeSubwindowSettings(subwindow_settings); + initializePawnPaths(pawn_paths); + } - // Mark sensor types already added - present_sensors_bitmask |= 1U << Utils::toNumeric(sensor_type); - } - } + // returns number of warnings + void load(std::function simmode_getter) { + warning_messages.clear(); + error_messages.clear(); + const Settings &settings_json = Settings::singleton(); + checkSettingsVersion(settings_json); - // Only add default sensors which are not present - for (const auto &p : sensor_defaults) { - auto type = Utils::toNumeric(p.second->sensor_type); + loadCoreSimModeSettings(settings_json, simmode_getter); + loadLevelSettings(settings_json); + loadDefaultCameraSetting(settings_json, camera_defaults); + loadCameraDirectorSetting(settings_json, camera_director, simmode_name); + loadSubWindowsSettings(settings_json, subwindow_settings); + loadViewModeSettings(settings_json); + loadSegmentationSetting(settings_json, segmentation_setting); + loadPawnPaths(settings_json, pawn_paths); + loadOtherSettings(settings_json); + loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults); + loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults, camera_defaults); + loadExternalCameraSettings(settings_json, external_cameras, camera_defaults); - if ((present_sensors_bitmask & (1U << type)) == 0) - sensors[p.first] = p.second; - } + // this should be done last because it depends on vehicles (and/or their type) we have + loadRecordingSetting(settings_json); + loadClockSettings(settings_json); } - // creates default sensor list when none specified in json - static void createDefaultSensorSettings(const std::string &simmode_name, - std::map> &sensors) { - if (simmode_name == kSimModeTypeMultirotor) { - sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true); - sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true); - sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); - sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true); - } else if (simmode_name == kSimModeTypeCar) { - sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true); - } else { - // no sensors added for other modes - } + static void initializeSettings(const std::string &json_settings_text) { + singleton().settings_text_ = json_settings_text; + Settings &settings_json = Settings::loadJSonString(json_settings_text); + if (!settings_json.isLoadSuccess()) + throw std::invalid_argument("Cannot parse JSON settings_json string."); } - // loads or creates default sensor list - static void loadDefaultSensorSettings(const std::string &simmode_name, const Settings &settings_json, - std::map> &sensors) { - nervosys::autonomylib::Settings sensors_child; - if (settings_json.getChild("DefaultSensors", sensors_child)) - loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors); - else - createDefaultSensorSettings(simmode_name, sensors); + static void createDefaultSettingsFile() { + initializeSettings("{}"); + + Settings &settings_json = Settings::singleton(); + // write some settings_json in new file otherwise the string "null" is written if all settings_json are empty + settings_json.setString("SeeDocsAt", "https://github.com/nervosys/AutonomySim/blob/master/docs/settings.md"); + settings_json.setDouble("SettingsVersion", 1.2); + + std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); + // TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only + // support Unreal 4.17 + // https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html + settings_json.saveJSonFile(settings_filename); } - static void loadExternalCameraSettings(const Settings &settings_json, CameraSettingMap &external_cameras, - const CameraSetting &camera_defaults) { - external_cameras.clear(); + // This is for the case when a new vehicle is made on the fly, at runtime + void addVehicleSetting(const std::string &vehicle_name, const std::string &vehicle_type, const Pose &pose, + const std::string &pawn_path = "") { + // No Mavlink-type vehicles currently + auto vehicle_setting = std::unique_ptr(new VehicleSetting(vehicle_name, vehicle_type)); + vehicle_setting->position = pose.position; + vehicle_setting->pawn_path = pawn_path; - Settings json_parent; - if (settings_json.getChild("ExternalCameras", json_parent)) { - std::vector keys; - json_parent.getChildNames(keys); + vehicle_setting->sensors = sensor_defaults; - for (const auto &key : keys) { - Settings child; - json_parent.getChild(key, child); - external_cameras[key] = createCameraSetting(child, camera_defaults); - } - } + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, + vehicle_setting->rotation.yaw); + + vehicles[vehicle_name] = std::move(vehicle_setting); + } + + const VehicleSetting *getVehicleSetting(const std::string &vehicle_name) const { + auto it = vehicles.find(vehicle_name); + if (it == vehicles.end()) + // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight' + it = vehicles.find("SimpleFlight"); + return it->second.get(); + } + + static Vector3r createVectorSetting(const Settings &settings_json, const Vector3r &default_vec) { + return Vector3r(settings_json.getFloat("X", default_vec.x()), settings_json.getFloat("Y", default_vec.y()), + settings_json.getFloat("Z", default_vec.z())); + } + static Rotation createRotationSetting(const Settings &settings_json, const Rotation &default_rot) { + return Rotation(settings_json.getFloat("Yaw", default_rot.yaw), + settings_json.getFloat("Pitch", default_rot.pitch), + settings_json.getFloat("Roll", default_rot.roll)); } }; diff --git a/AutonomyLib/include/common/CancelToken.hpp b/AutonomyLib/include/common/CancelToken.hpp index e22bc954..1b7c6b8c 100644 --- a/AutonomyLib/include/common/CancelToken.hpp +++ b/AutonomyLib/include/common/CancelToken.hpp @@ -4,8 +4,9 @@ #ifndef autonomylib_common_CancelToken_hpp #define autonomylib_common_CancelToken_hpp -#include "common/Common.hpp" -#include "common/utils/Utils.hpp" +#include "Common.hpp" +#include "utils/Utils.hpp" + #include #include @@ -13,6 +14,14 @@ namespace nervosys { namespace autonomylib { class CancelToken { + + private: + std::atomic is_cancelled_; + std::atomic is_complete_; + std::atomic recursion_count_; + + std::recursive_mutex wait_mutex_; + public: CancelToken() : is_cancelled_(false), is_complete_(false), recursion_count_(0) {} @@ -65,13 +74,6 @@ class CancelToken { wait_mutex_.lock(); ++recursion_count_; } - - private: - std::atomic is_cancelled_; - std::atomic is_complete_; - std::atomic recursion_count_; - - std::recursive_mutex wait_mutex_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/ClockBase.hpp b/AutonomyLib/include/common/ClockBase.hpp index e0dd769c..02c893e9 100644 --- a/AutonomyLib/include/common/ClockBase.hpp +++ b/AutonomyLib/include/common/ClockBase.hpp @@ -5,6 +5,7 @@ #define autonomylib_common_ClockBase_hpp #include "Common.hpp" + #include #include @@ -12,6 +13,13 @@ namespace nervosys { namespace autonomylib { class ClockBase { + + private: + template using duration = std::chrono::duration; + + uint64_t step_count_ = 0; + TTimePoint wall_clock_start_; + public: // returns value indicating nanoseconds elapsed since some reference timepoint in history // typically nanoseconds from Unix epoch @@ -68,12 +76,6 @@ class ClockBase { return static_cast(clock_elapsed) / wall_clock_elapsed; } - - private: - template using duration = std::chrono::duration; - - uint64_t step_count_ = 0; - TTimePoint wall_clock_start_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/ClockFactory.hpp b/AutonomyLib/include/common/ClockFactory.hpp index 55d9981e..b5691281 100644 --- a/AutonomyLib/include/common/ClockFactory.hpp +++ b/AutonomyLib/include/common/ClockFactory.hpp @@ -5,12 +5,18 @@ #define autonomylib_common_ClockFactory_hpp #include "ScalableClock.hpp" + #include namespace nervosys { namespace autonomylib { class ClockFactory { + + private: + // disallow instance creation + ClockFactory() {} + public: // output of this function should not be stored as pointer might change static ClockBase *get(std::shared_ptr val = nullptr) { @@ -28,10 +34,6 @@ class ClockFactory { // don't allow multiple instances of this class ClockFactory(ClockFactory const &) = delete; void operator=(ClockFactory const &) = delete; - - private: - // disallow instance creation - ClockFactory() {} }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/Common.hpp b/AutonomyLib/include/common/Common.hpp index 92f7fba8..e62c9523 100644 --- a/AutonomyLib/include/common/Common.hpp +++ b/AutonomyLib/include/common/Common.hpp @@ -13,8 +13,8 @@ #include #include "VectorMath.hpp" -#include "common/utils/RandomGenerator.hpp" -#include "common/utils/Utils.hpp" +#include "utils/RandomGenerator.hpp" +#include "utils/Utils.hpp" #ifndef _CRT_SECURE_NO_WARNINGS #define _CRT_SECURE_NO_WARNINGS 1 diff --git a/AutonomyLib/include/common/CommonStructs.hpp b/AutonomyLib/include/common/CommonStructs.hpp index b829ae4f..3d66e11a 100644 --- a/AutonomyLib/include/common/CommonStructs.hpp +++ b/AutonomyLib/include/common/CommonStructs.hpp @@ -6,7 +6,7 @@ #include -#include "common/Common.hpp" +#include "Common.hpp" namespace nervosys { namespace autonomylib { diff --git a/AutonomyLib/include/common/DelayLine.hpp b/AutonomyLib/include/common/DelayLine.hpp index 592cb748..66311c00 100644 --- a/AutonomyLib/include/common/DelayLine.hpp +++ b/AutonomyLib/include/common/DelayLine.hpp @@ -4,22 +4,32 @@ #ifndef autonomylib_common_DelayLine_hpp #define autonomylib_common_DelayLine_hpp +#include "Common.hpp" #include "UpdatableObject.hpp" -#include "common/Common.hpp" + #include namespace nervosys { namespace autonomylib { template class DelayLine : public UpdatableObject { + + private: + template using list = std::list; + + list values_; + list times_; + TTimeDelta delay_; + + T last_value_; + TTimePoint last_time_; + public: DelayLine() {} - DelayLine(TTimeDelta delay) // in seconds - { + DelayLine(TTimeDelta delay) { // in seconds initialize(delay); } - void initialize(TTimeDelta delay) // in seconds - { + void initialize(TTimeDelta delay) { // in seconds setDelay(delay); } void setDelay(TTimeDelta delay) { delay_ = delay; } @@ -54,16 +64,6 @@ template class DelayLine : public UpdatableObject { values_.push_back(val); times_.push_back(clock()->nowNanos() + time_offset); } - - private: - template using list = std::list; - - list values_; - list times_; - TTimeDelta delay_; - - T last_value_; - TTimePoint last_time_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/EarthCelestial.hpp b/AutonomyLib/include/common/EarthCelestial.hpp index 82845489..0da0b457 100644 --- a/AutonomyLib/include/common/EarthCelestial.hpp +++ b/AutonomyLib/include/common/EarthCelestial.hpp @@ -6,8 +6,9 @@ Adopted from SunCalc by Vladimir Agafonkin #ifndef autonomylib_common_EarthCelestial_hpp #define autonomylib_common_EarthCelestial_hpp +#include "Common.hpp" #include "EarthUtils.hpp" -#include "common/Common.hpp" + #include #include @@ -15,6 +16,87 @@ namespace nervosys { namespace autonomylib { class EarthCelestial { + + private: + static double toDays(uint64_t date) { + static constexpr double kJulianDaysOnY2000 = 2451545; + static constexpr double kDaysToHours = 60 * 60 * 24; + static constexpr double kJulianDaysOnEpoch = 2440588; + + double julian_days = date / kDaysToHours - 0.5 + kJulianDaysOnEpoch; + ; + return julian_days - kJulianDaysOnY2000; + } + + static double rightAscension(double l, double b) { + return std::atan2(std::sin(l) * std::cos(EarthUtils::Obliquity) - std::tan(b) * std::sin(EarthUtils::Obliquity), + std::cos(l)); + } + + static double declination(double l, double b) { + return std::asin(std::sin(b) * std::cos(EarthUtils::Obliquity) + + std::cos(b) * std::sin(EarthUtils::Obliquity) * std::sin(l)); + } + + static double azimuth(double H, double phi, double declination) { + return std::atan2(std::sin(H), std::cos(H) * std::sin(phi) - std::tan(declination) * std::cos(phi)); + } + + static double altitude(double H, double phi, double declination) { + return std::asin(std::sin(phi) * std::sin(declination) + std::cos(phi) * std::cos(declination) * std::cos(H)); + } + + static double siderealTime(double d, double lw) { return Utils::degreesToRadians((280.16 + 360.9856235 * d)) - lw; } + + static double astroRefraction(double h) { + if (h < 0) // the following formula works for positive altitudes only. + h = 0; // if h = -0.08901179 a div/0 would occur. + + // formula 16.4 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998. + // 1.02 / tan(h + 10.26 / (h + 5.10)) h in degrees, result in arc minutes -> converted to rad: + return 0.0002967 / std::tan(h + 0.00312536 / (h + 0.08901179)); + } + + static double solarMeanAnomaly(double d) { return Utils::degreesToRadians((357.5291 + 0.98560028 * d)); } + + static double eclipticLongitude(double M) { + double C = Utils::degreesToRadians( + (1.9148 * std::sin(M) + 0.02 * std::sin(2 * M) + 0.0003 * std::sin(3 * M))); // equation of center + + return M + C + EarthUtils::Perihelion + M_PI; + } + + static CelestialGlobalCoord getGlobalSunCoords(double d) { + double M = solarMeanAnomaly(d); + double L = eclipticLongitude(M); + + CelestialGlobalCoord sunCoords; + sunCoords.declination = declination(L, 0); + sunCoords.rightAscension = rightAscension(L, 0); + + return sunCoords; + } + + // moon calculations, based on http://aa.quae.nl/en/reken/hemelpositie.html formulas + static CelestialGlobalCoord getGlobalMoonCoords(double d) { + // geocentric ecliptic coordinates of the moon + + double L = Utils::degreesToRadians((218.316 + 13.176396 * d)); // ecliptic longitude + double M = Utils::degreesToRadians((134.963 + 13.064993 * d)); // mean anomaly + double F = Utils::degreesToRadians((93.272 + 13.229350 * d)); // mean distance + + double l = L + Utils::degreesToRadians(6.289 * std::sin(M)); // longitude + double b = Utils::degreesToRadians(5.128 * std::sin(F)); // latitude + double dt = 385001 - 20905 * std::cos(M); // distance to the moon in km + + CelestialGlobalCoord moonCoords; + moonCoords.rightAscension = rightAscension(l, b); + moonCoords.declination = declination(l, b); + moonCoords.distance = dt; + + return moonCoords; + } + public: struct CelestialGlobalCoord { double declination; @@ -36,7 +118,6 @@ class EarthCelestial { double angle; }; - public: static CelestialLocalCoord getSunCoordinates(uint64_t date, double lat, double lng) { double lw = Utils::degreesToRadians(-lng); double phi = Utils::degreesToRadians(lat); @@ -101,86 +182,6 @@ class EarthCelestial { moonPhase.angle = angle; return moonPhase; }; - - private: - static double toDays(uint64_t date) { - static constexpr double kJulianDaysOnY2000 = 2451545; - static constexpr double kDaysToHours = 60 * 60 * 24; - static constexpr double kJulianDaysOnEpoch = 2440588; - - double julian_days = date / kDaysToHours - 0.5 + kJulianDaysOnEpoch; - ; - return julian_days - kJulianDaysOnY2000; - } - - static double rightAscension(double l, double b) { - return std::atan2(std::sin(l) * std::cos(EarthUtils::Obliquity) - std::tan(b) * std::sin(EarthUtils::Obliquity), - std::cos(l)); - } - - static double declination(double l, double b) { - return std::asin(std::sin(b) * std::cos(EarthUtils::Obliquity) + - std::cos(b) * std::sin(EarthUtils::Obliquity) * std::sin(l)); - } - - static double azimuth(double H, double phi, double declination) { - return std::atan2(std::sin(H), std::cos(H) * std::sin(phi) - std::tan(declination) * std::cos(phi)); - } - - static double altitude(double H, double phi, double declination) { - return std::asin(std::sin(phi) * std::sin(declination) + std::cos(phi) * std::cos(declination) * std::cos(H)); - } - - static double siderealTime(double d, double lw) { return Utils::degreesToRadians((280.16 + 360.9856235 * d)) - lw; } - - static double astroRefraction(double h) { - if (h < 0) // the following formula works for positive altitudes only. - h = 0; // if h = -0.08901179 a div/0 would occur. - - // formula 16.4 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998. - // 1.02 / tan(h + 10.26 / (h + 5.10)) h in degrees, result in arc minutes -> converted to rad: - return 0.0002967 / std::tan(h + 0.00312536 / (h + 0.08901179)); - } - - static double solarMeanAnomaly(double d) { return Utils::degreesToRadians((357.5291 + 0.98560028 * d)); } - - static double eclipticLongitude(double M) { - double C = Utils::degreesToRadians( - (1.9148 * std::sin(M) + 0.02 * std::sin(2 * M) + 0.0003 * std::sin(3 * M))); // equation of center - - return M + C + EarthUtils::Perihelion + M_PI; - } - - static CelestialGlobalCoord getGlobalSunCoords(double d) { - double M = solarMeanAnomaly(d); - double L = eclipticLongitude(M); - - CelestialGlobalCoord sunCoords; - sunCoords.declination = declination(L, 0); - sunCoords.rightAscension = rightAscension(L, 0); - - return sunCoords; - } - - // moon calculations, based on http://aa.quae.nl/en/reken/hemelpositie.html formulas - static CelestialGlobalCoord getGlobalMoonCoords(double d) { - // geocentric ecliptic coordinates of the moon - - double L = Utils::degreesToRadians((218.316 + 13.176396 * d)); // ecliptic longitude - double M = Utils::degreesToRadians((134.963 + 13.064993 * d)); // mean anomaly - double F = Utils::degreesToRadians((93.272 + 13.229350 * d)); // mean distance - - double l = L + Utils::degreesToRadians(6.289 * std::sin(M)); // longitude - double b = Utils::degreesToRadians(5.128 * std::sin(F)); // latitude - double dt = 385001 - 20905 * std::cos(M); // distance to the moon in km - - CelestialGlobalCoord moonCoords; - moonCoords.rightAscension = rightAscension(l, b); - moonCoords.declination = declination(l, b); - moonCoords.distance = dt; - - return moonCoords; - } }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/EarthUtils.hpp b/AutonomyLib/include/common/EarthUtils.hpp index 0f746bca..a63d7eab 100644 --- a/AutonomyLib/include/common/EarthUtils.hpp +++ b/AutonomyLib/include/common/EarthUtils.hpp @@ -4,8 +4,9 @@ #ifndef autonomylib_common_EarthUtils_hpp #define autonomylib_common_EarthUtils_hpp -#include "common/Common.hpp" -#include "common/CommonStructs.hpp" +#include "Common.hpp" +#include "CommonStructs.hpp" + #include #include @@ -13,6 +14,7 @@ namespace nervosys { namespace autonomylib { class EarthUtils { + private: /** set this always to the sampling in degrees for the table below */ static constexpr int MAG_SAMPLING_RES = 10; @@ -50,6 +52,11 @@ class EarthUtils { 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3}, }; + /* magnetic field */ + static float get_mag_lookup_table_val(int lat_index, int lon_index) { + return static_cast(DECLINATION_TABLE[lat_index][lon_index]); + } + public: // return declination in degrees // Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp @@ -149,8 +156,7 @@ class EarthUtils { return getStandardPressure(geopot_height, t); } - static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) // return Pa - { + static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) { // return Pa // Below 51km: Practical Meteorology by Roland Stull, pg 12 // Above 51km: http://www.braeunig.us/space/atmmodel.htm // Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf @@ -177,22 +183,19 @@ class EarthUtils { // throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!"); } - static real_T getAirDensity(real_T std_pressure, real_T std_temperature) // kg / m^3 - { + static real_T getAirDensity(real_T std_pressure, real_T std_temperature) { // kg / m^3 // http://www.braeunig.us/space/atmmodel.htm return std_pressure / 287.053f / std_temperature; } - static real_T getAirDensity(real_T altitude) // kg / m^3 - { + static real_T getAirDensity(real_T altitude) { // kg / m^3 real_T geo_pot = getGeopotential(altitude / 1000.0f); real_T std_temperature = getStandardTemperature(geo_pot); real_T std_pressure = getStandardPressure(geo_pot, std_temperature); return getAirDensity(std_pressure, std_temperature); } - static real_T getSpeedofSound(real_T altitude) // m/s - { + static real_T getSpeedofSound(real_T altitude) { // m/s // http://www.braeunig.us/space/atmmodel.htm return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude))); } @@ -209,43 +212,41 @@ class EarthUtils { } } - static Vector3r getMagField(const GeoPoint &geo_point) // return Tesla - { + static Vector3r getMagField(const GeoPoint &geo_point) { // return Tesla double declination, inclination; return getMagField(geo_point, declination, inclination); } - static Vector3r getMagField(const GeoPoint &geo_point, double &declination, double &inclination) // return Tesla - { + static Vector3r getMagField(const GeoPoint &geo_point, double &declination, double &inclination) { // return Tesla /* - We calculate magnetic field using simple dipol model of Earth, i.e., assume - earth as perfect dipole sphere and ignoring all but first order terms. - This obviously is inaccurate because of huge amount of irregularities, magnetic pole that is - constantly moving, shape of Earth, higher order terms, dipole that is not perfectly aligned etc. - For simulation we are not looking for actual values of magnetic field but rather if field changes - correctly as vehicle moves in any direction and if field component signs are correct. For this purpose, simple - diapole model is good enough. Keep in mind that actual field values may differ by as much as 10X in either direction - although for many tests differences seems to be within 3X or sometime even to first decimal digit. Again what - matters is how field changes wrt to movement as opposed to actual field values. To get better field strength one - should use latest World Magnetic Model like WMM2015 from NOAA. However these recent model is fairly complex and very - expensive to calculate. Other possibilities: - - WMM2010 mocel, expensive to compute: http://williams.best.vwh.net/magvar/magfield.c - - Android's mag field calculation (still uses WMM2010 and fails at North Pole): https://goo.gl/1CZB9x - - Performance: - This function takes about 1 microsecond on Lenovo P50 laptop (Intel Xeon E3-1505M v5 CPU) - Basic trignometry functions runs at 30ns. - - Accuracy: - Two points separated by sqrt(2 km) - Dipole Model: 2.50394e-05 3.40771e-06 3.6567e-05 (dec: 7.7500, inc: 55.3530) - WMM2015 Model: 1.8350e-05 5.201e-06 5.0158e-05 (dec: 15.8248, inc: 69.1805) - geo: 47.637 -122.147 622 - - Dipole Model: 2.5047e-05 3.41024e-06 3.65953e-05 (dec: 7.7536, inc: 55.36532) - WMM2015 Model: 1.8353e-05 5.203e-06 5.0191e-05 (dec: 15.8278, inc: 69.1897) - geo: 47.646 -122.134 -378 - */ + We calculate magnetic field using simple dipol model of Earth, i.e., assume + earth as perfect dipole sphere and ignoring all but first order terms. + This obviously is inaccurate because of huge amount of irregularities, magnetic pole that is + constantly moving, shape of Earth, higher order terms, dipole that is not perfectly aligned etc. + For simulation we are not looking for actual values of magnetic field but rather if field changes + correctly as vehicle moves in any direction and if field component signs are correct. For this purpose, simple + diapole model is good enough. Keep in mind that actual field values may differ by as much as 10X in either + direction although for many tests differences seems to be within 3X or sometime even to first decimal digit. + Again what matters is how field changes wrt to movement as opposed to actual field values. To get better field + strength one should use latest World Magnetic Model like WMM2015 from NOAA. However these recent model is fairly + complex and very expensive to calculate. Other possibilities: + - WMM2010 mocel, expensive to compute: http://williams.best.vwh.net/magvar/magfield.c + - Android's mag field calculation (still uses WMM2010 and fails at North Pole): https://goo.gl/1CZB9x + + Performance: + This function takes about 1 microsecond on Lenovo P50 laptop (Intel Xeon E3-1505M v5 CPU) + Basic trignometry functions runs at 30ns. + + Accuracy: + Two points separated by sqrt(2 km) + Dipole Model: 2.50394e-05 3.40771e-06 3.6567e-05 (dec: 7.7500, inc: 55.3530) + WMM2015 Model: 1.8350e-05 5.201e-06 5.0158e-05 (dec: 15.8248, inc: 69.1805) + geo: 47.637 -122.147 622 + + Dipole Model: 2.5047e-05 3.41024e-06 3.65953e-05 (dec: 7.7536, inc: 55.36532) + WMM2015 Model: 1.8353e-05 5.203e-06 5.0191e-05 (dec: 15.8278, inc: 69.1897) + geo: 47.646 -122.134 -378 + */ // ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni // Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt @@ -372,7 +373,7 @@ class EarthUtils { return r; } - public: // consts + // constants // ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f); static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f); @@ -386,12 +387,6 @@ class EarthUtils { static constexpr float Obliquity = Utils::degreesToRadians(23.4397f); static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth static constexpr double DistanceFromSun = 149597870700.0; // meters - - private: - /* magnetic field */ - static float get_mag_lookup_table_val(int lat_index, int lon_index) { - return static_cast(DECLINATION_TABLE[lat_index][lon_index]); - } }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/FirstOrderFilter.hpp b/AutonomyLib/include/common/FirstOrderFilter.hpp index db52de20..bc8a18c8 100644 --- a/AutonomyLib/include/common/FirstOrderFilter.hpp +++ b/AutonomyLib/include/common/FirstOrderFilter.hpp @@ -6,6 +6,7 @@ #include "Common.hpp" #include "UpdatableObject.hpp" + #include namespace nervosys { @@ -13,17 +14,24 @@ namespace autonomylib { template class FirstOrderFilter : public UpdatableObject { /* -This class can be used to apply a first order filter on a signal. -It allows different acceleration and deceleration time constants. - -Short review of discrete time implementation of first order system: -Laplace: -X(s)/U(s) = 1/(tau*s + 1) -continuous time system: -dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) -discretized system (ZoH): -x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) -*/ + This class can be used to apply a first order filter on a signal. + It allows different acceleration and deceleration time constants. + + Short review of discrete time implementation of first order system: + Laplace: + X(s)/U(s) = 1/(tau*s + 1) + continuous time system: + dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) + discretized system (ZoH): + x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) + */ + + private: + float timeConstant_; + T output_, input_; + T initial_output_, initial_input_; + TTimePoint last_time_; + public: FirstOrderFilter() { // allow default constructor with later call for initialize @@ -61,12 +69,6 @@ x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u( T getInput() const { return input_; } T getOutput() const { return output_; } - - private: - float timeConstant_; - T output_, input_; - T initial_output_, initial_input_; - TTimePoint last_time_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/FrequencyLimiter.hpp b/AutonomyLib/include/common/FrequencyLimiter.hpp index a91f98f9..34461d5c 100644 --- a/AutonomyLib/include/common/FrequencyLimiter.hpp +++ b/AutonomyLib/include/common/FrequencyLimiter.hpp @@ -4,13 +4,26 @@ #ifndef autonomylib_common_FrequencyLimiter_hpp #define autonomylib_common_FrequencyLimiter_hpp +#include "Common.hpp" #include "UpdatableObject.hpp" -#include "common/Common.hpp" namespace nervosys { namespace autonomylib { class FrequencyLimiter : public UpdatableObject { + + private: + real_T interval_size_sec_; + TTimeDelta elapsed_total_sec_; + TTimeDelta elapsed_interval_sec_; + TTimeDelta last_elapsed_interval_sec_; + uint update_count_; + real_T frequency_; + real_T startup_delay_; + bool interval_complete_; + bool startup_complete_; + TTimePoint last_time_, first_time_; + public: FrequencyLimiter(real_T frequency = Utils::max(), real_T startup_delay = 0) { initialize(frequency, startup_delay); @@ -88,18 +101,6 @@ class FrequencyLimiter : public UpdatableObject { bool isStartupComplete() const { return startup_complete_; } uint getUpdateCount() const { return update_count_; } - - private: - real_T interval_size_sec_; - TTimeDelta elapsed_total_sec_; - TTimeDelta elapsed_interval_sec_; - TTimeDelta last_elapsed_interval_sec_; - uint update_count_; - real_T frequency_; - real_T startup_delay_; - bool interval_complete_; - bool startup_complete_; - TTimePoint last_time_, first_time_; }; } // namespace autonomylib } // namespace nervosys diff --git a/AutonomyLib/include/common/GaussianMarkov.hpp b/AutonomyLib/include/common/GaussianMarkov.hpp index beb0cea9..e109d4b5 100644 --- a/AutonomyLib/include/common/GaussianMarkov.hpp +++ b/AutonomyLib/include/common/GaussianMarkov.hpp @@ -4,15 +4,23 @@ #ifndef autonomylib_common_GaussianMarkov_hpp #define autonomylib_common_GaussianMarkov_hpp +#include "Common.hpp" #include "UpdatableObject.hpp" -#include "common/Common.hpp" -#include "common/utils/RandomGenerator.hpp" +#include "utils/RandomGenerator.hpp" + #include namespace nervosys { namespace autonomylib { class GaussianMarkov : public UpdatableObject { + + private: + RandomGeneratorGausianR rand_; + real_T tau_, sigma_; + real_T output_, initial_output_; + TTimePoint last_time_; + public: GaussianMarkov() {} GaussianMarkov(real_T tau, real_T sigma, real_T initial_output = 0) { // in seconds @@ -56,12 +64,6 @@ class GaussianMarkov : public UpdatableObject { real_T getNextRandom() { return rand_.next(); } real_T getOutput() const { return output_; } - - private: - RandomGeneratorGausianR rand_; - real_T tau_, sigma_; - real_T output_, initial_output_; - TTimePoint last_time_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/GeodeticConverter.hpp b/AutonomyLib/include/common/GeodeticConverter.hpp index d694c70b..6afc498d 100644 --- a/AutonomyLib/include/common/GeodeticConverter.hpp +++ b/AutonomyLib/include/common/GeodeticConverter.hpp @@ -5,12 +5,61 @@ #define autonomylib_common_GeodeticConverter_hpp #include "VectorMath.hpp" + #include namespace nervosys { namespace autonomylib { class GeodeticConverter { + + private: + typedef nervosys::autonomylib::VectorMathf VectorMath; + typedef VectorMath::Vector3d Vector3d; + typedef VectorMath::Matrix3x3d Matrix3x3d; + + // Geodetic system parameters + static constexpr double kSemimajorAxis = 6378137; + static constexpr double kSemiminorAxis = 6356752.3142; + static constexpr double kFirstEccentricitySquared = 6.69437999014 * 0.001; + static constexpr double kSecondEccentricitySquared = 6.73949674228 * 0.001; + static constexpr double kFlattening = 1 / 298.257223563; + + inline Matrix3x3d nRe(const double lat_radians, const double lon_radians) { + const double sLat = sin(lat_radians); + const double sLon = sin(lon_radians); + const double cLat = cos(lat_radians); + const double cLon = cos(lon_radians); + + Matrix3x3d ret; + ret(0, 0) = -sLat * cLon; + ret(0, 1) = -sLat * sLon; + ret(0, 2) = cLat; + ret(1, 0) = -sLon; + ret(1, 1) = cLon; + ret(1, 2) = 0.0; + ret(2, 0) = cLat * cLon; + ret(2, 1) = cLat * sLon; + ret(2, 2) = sLat; + + return ret; + } + + inline double rad2Deg(const double radians) { return (radians / M_PI) * 180.0; } + + inline double deg2Rad(const double degrees) { return (degrees / 180.0) * M_PI; } + + double home_latitude_rad_, home_latitude_; + double home_longitude_rad_, home_longitude_; + float home_altitude_; + + double home_ecef_x_; + double home_ecef_y_; + double home_ecef_z_; + + Matrix3x3d ecef_to_ned_matrix_; + Matrix3x3d ned_to_ecef_matrix_; + public: GeodeticConverter(double home_latitude = 0, double home_longitude = 0, float home_altitude = 0) { setHome(home_latitude, home_longitude, home_altitude); @@ -155,53 +204,6 @@ class GeodeticConverter { ecef2Geodetic(x, y, z, latitude, longitude, altitude); } - private: - typedef nervosys::autonomylib::VectorMathf VectorMath; - typedef VectorMath::Vector3d Vector3d; - typedef VectorMath::Matrix3x3d Matrix3x3d; - - // Geodetic system parameters - static constexpr double kSemimajorAxis = 6378137; - static constexpr double kSemiminorAxis = 6356752.3142; - static constexpr double kFirstEccentricitySquared = 6.69437999014 * 0.001; - static constexpr double kSecondEccentricitySquared = 6.73949674228 * 0.001; - static constexpr double kFlattening = 1 / 298.257223563; - - inline Matrix3x3d nRe(const double lat_radians, const double lon_radians) { - const double sLat = sin(lat_radians); - const double sLon = sin(lon_radians); - const double cLat = cos(lat_radians); - const double cLon = cos(lon_radians); - - Matrix3x3d ret; - ret(0, 0) = -sLat * cLon; - ret(0, 1) = -sLat * sLon; - ret(0, 2) = cLat; - ret(1, 0) = -sLon; - ret(1, 1) = cLon; - ret(1, 2) = 0.0; - ret(2, 0) = cLat * cLon; - ret(2, 1) = cLat * sLon; - ret(2, 2) = sLat; - - return ret; - } - - inline double rad2Deg(const double radians) { return (radians / M_PI) * 180.0; } - - inline double deg2Rad(const double degrees) { return (degrees / 180.0) * M_PI; } - - double home_latitude_rad_, home_latitude_; - double home_longitude_rad_, home_longitude_; - float home_altitude_; - - double home_ecef_x_; - double home_ecef_y_; - double home_ecef_z_; - - Matrix3x3d ecef_to_ned_matrix_; - Matrix3x3d ned_to_ecef_matrix_; - }; // class GeodeticConverter } // namespace autonomylib diff --git a/AutonomyLib/include/common/ImageCaptureBase.hpp b/AutonomyLib/include/common/ImageCaptureBase.hpp index 23b27daf..39deaae2 100644 --- a/AutonomyLib/include/common/ImageCaptureBase.hpp +++ b/AutonomyLib/include/common/ImageCaptureBase.hpp @@ -4,14 +4,19 @@ #ifndef autonomylib_common_ImageCaptureBase_hpp #define autonomylib_common_ImageCaptureBase_hpp -#include "common/Common.hpp" -#include "common/utils/EnumFlags.hpp" +#include "Common.hpp" +#include "utils/EnumFlags.hpp" namespace nervosys { namespace autonomylib { // This is an abstraction for cameras associated with a vehicle. Each camera has a unique id. class ImageCaptureBase { + + public: // methods + virtual void getImages(const std::vector &requests, std::vector &responses) const = 0; + virtual ~ImageCaptureBase() = default; + public: // types enum class ImageType : int { // this indexes to array, -1 is special to indicate main camera Scene = 0, @@ -55,10 +60,6 @@ class ImageCaptureBase { int width = 0, height = 0; ImageType image_type; }; - - public: // methods - virtual void getImages(const std::vector &requests, std::vector &responses) const = 0; - virtual ~ImageCaptureBase() = default; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/LogFileWriter.hpp b/AutonomyLib/include/common/LogFileWriter.hpp index e03e3699..f5f19ed1 100644 --- a/AutonomyLib/include/common/LogFileWriter.hpp +++ b/AutonomyLib/include/common/LogFileWriter.hpp @@ -4,7 +4,8 @@ #ifndef autonomylib_common_LogFileWriter_hpp #define autonomylib_common_LogFileWriter_hpp -#include "common/Common.hpp" +#include "Common.hpp" + #include #include @@ -12,6 +13,12 @@ namespace nervosys { namespace autonomylib { class LogFileWriter { + + private: + std::ofstream log_file_; + string file_name_; + bool enabled_ = false; + public: LogFileWriter() {} LogFileWriter(const string &file_name, bool enabled = true) { open(file_name, enabled); } @@ -50,11 +57,6 @@ class LogFileWriter { log_file_ << std::endl; } } - - private: - std::ofstream log_file_; - string file_name_; - bool enabled_ = false; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/PidController.hpp b/AutonomyLib/include/common/PidController.hpp index 508677fe..73f04149 100644 --- a/AutonomyLib/include/common/PidController.hpp +++ b/AutonomyLib/include/common/PidController.hpp @@ -15,6 +15,7 @@ namespace autonomylib { // control output needed to achieve the setPoint goal. Integration is done using // dt measured using system clock. class PidController { + private: float set_point_; float kProportional_; diff --git a/AutonomyLib/include/common/ScalableClock.hpp b/AutonomyLib/include/common/ScalableClock.hpp index a27ccc67..d4ec30ab 100644 --- a/AutonomyLib/include/common/ScalableClock.hpp +++ b/AutonomyLib/include/common/ScalableClock.hpp @@ -12,6 +12,24 @@ namespace autonomylib { // ScalableClock is a clock that can be scaled (i.e. slowed down or speeded up) class ScalableClock : public ClockBase { + + private: + double scale_; + TTimeDelta latency_; + double offset_; + TTimePoint start_; + + protected: + // converts time interval for wall clock to current clock + // For example, if implementation is scaled clock simulating 5X spped then below + // will retun dt*5. This functions are required to translate time to operating system + // which only has concept of wall clock. For example, to make thread sleep for specific duration. + + // wall clock to sim clock + virtual TTimeDelta fromWallDelta(TTimeDelta dt) const { return dt * scale_; } + // sim clock to wall clock + virtual TTimeDelta toWallDelta(TTimeDelta dt) const { return dt / scale_; } + public: // scale > 1 slows down the clock, < 1 speeds up the clock ScalableClock(double scale = 1, TTimeDelta latency = 0) : scale_(scale), latency_(latency) { @@ -48,23 +66,6 @@ class ScalableClock : public ClockBase { } else ClockBase::sleep_for(dt); } - - protected: - // converts time interval for wall clock to current clock - // For example, if implementation is scaled clock simulating 5X spped then below - // will retun dt*5. This functions are required to translate time to operating system - // which only has concept of wall clock. For example, to make thread sleep for specific duration. - - // wall clock to sim clock - virtual TTimeDelta fromWallDelta(TTimeDelta dt) const { return dt * scale_; } - // sim clock to wall clock - virtual TTimeDelta toWallDelta(TTimeDelta dt) const { return dt / scale_; } - - private: - double scale_; - TTimeDelta latency_; - double offset_; - TTimePoint start_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/Settings.hpp b/AutonomyLib/include/common/Settings.hpp index 7594d4cd..a7e7f4c7 100644 --- a/AutonomyLib/include/common/Settings.hpp +++ b/AutonomyLib/include/common/Settings.hpp @@ -7,28 +7,28 @@ #include #include -#include "common/utils/Utils.hpp" +#include "utils/Utils.hpp" STRICT_MODE_OFF // this json library is not strict clean // TODO: HACK!! below are added temporariliy because something is defining min, max macros // #undef max #undef min -#include "common/utils/json.hpp" +#include "utils/json.hpp" STRICT_MODE_ON -#include "common/utils/FileSystem.hpp" +#include "utils/FileSystem.hpp" namespace nervosys { namespace autonomylib { class Settings { + private: std::string full_filepath_; nlohmann::json doc_; bool load_success_ = false; - private: static std::mutex &getFileAccessMutex() { static std::mutex file_access; return file_access; diff --git a/AutonomyLib/include/common/StateReporter.hpp b/AutonomyLib/include/common/StateReporter.hpp index 43f540d0..96afc43c 100644 --- a/AutonomyLib/include/common/StateReporter.hpp +++ b/AutonomyLib/include/common/StateReporter.hpp @@ -4,7 +4,8 @@ #ifndef autonomylib_common_StateReporter_hpp #define autonomylib_common_StateReporter_hpp -#include "common/Common.hpp" +#include "Common.hpp" + #include #include #include @@ -20,6 +21,13 @@ version is provided by StateReporterWrapper. We expect everyone to use StateReporterWrapper instead of StateReporter directly. */ class StateReporter { + + private: + std::stringstream ss_; + + int float_precision_ = 2; + bool is_scientific_notation_ = false; + public: StateReporter(int float_precision = 3, bool is_scientific_notation = false) { initialize(float_precision, is_scientific_notation); @@ -93,12 +101,6 @@ class StateReporter { ss_ << "\t"; } void endl() { ss_ << std::endl; } - - private: - std::stringstream ss_; - - int float_precision_ = 2; - bool is_scientific_notation_ = false; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/StateReporterWrapper.hpp b/AutonomyLib/include/common/StateReporterWrapper.hpp index e97d46ec..9795d47a 100644 --- a/AutonomyLib/include/common/StateReporterWrapper.hpp +++ b/AutonomyLib/include/common/StateReporterWrapper.hpp @@ -4,11 +4,12 @@ #ifndef autonomylib_common_StateReporterWrapper_hpp #define autonomylib_common_StateReporterWrapper_hpp +#include "Common.hpp" +#include "FrequencyLimiter.hpp" #include "StateReporter.hpp" #include "UpdatableObject.hpp" -#include "common/Common.hpp" -#include "common/FrequencyLimiter.hpp" -#include "common/utils/OnlineStats.hpp" +#include "utils/OnlineStats.hpp" + #include #include #include @@ -17,6 +18,19 @@ namespace nervosys { namespace autonomylib { class StateReporterWrapper : public UpdatableObject { + + private: + typedef common_utils::OnlineStats OnlineStats; + + StateReporter report_; + + OnlineStats dt_stats_; + + FrequencyLimiter report_freq_; + bool enabled_; + bool is_wait_complete = false; + TTimePoint last_time_; + public: static constexpr real_T DefaultReportFreq = 3.0f; @@ -90,18 +104,6 @@ class StateReporterWrapper : public UpdatableObject { report_freq_.initialize(0); } bool getEnable() { return enabled_; } - - private: - typedef common_utils::OnlineStats OnlineStats; - - StateReporter report_; - - OnlineStats dt_stats_; - - FrequencyLimiter report_freq_; - bool enabled_; - bool is_wait_complete = false; - TTimePoint last_time_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/SteppableClock.hpp b/AutonomyLib/include/common/SteppableClock.hpp index 33d9c3f0..48c1d98b 100644 --- a/AutonomyLib/include/common/SteppableClock.hpp +++ b/AutonomyLib/include/common/SteppableClock.hpp @@ -6,12 +6,19 @@ #include "ClockBase.hpp" #include "Common.hpp" + #include namespace nervosys { namespace autonomylib { class SteppableClock : public ClockBase { + + private: + std::atomic current_; + std::atomic start_; + TTimeDelta step_; + public: static constexpr real_T DefaultStepSize = 20E-3f; @@ -40,12 +47,6 @@ class SteppableClock : public ClockBase { TTimeDelta getStepSize() const { return step_; } virtual TTimePoint nowNanos() const override { return current_; } virtual TTimePoint getStart() const override { return start_; } - - private: - std::atomic current_; - std::atomic start_; - - TTimeDelta step_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/UpdatableContainer.hpp b/AutonomyLib/include/common/UpdatableContainer.hpp index 8efe4fdd..6ed0a7f7 100644 --- a/AutonomyLib/include/common/UpdatableContainer.hpp +++ b/AutonomyLib/include/common/UpdatableContainer.hpp @@ -4,13 +4,17 @@ #ifndef autonomylib_common_UpdatableContainer_hpp #define autonomylib_common_UpdatableContainer_hpp +#include "Common.hpp" #include "UpdatableObject.hpp" -#include "common/Common.hpp" namespace nervosys { namespace autonomylib { template class UpdatableContainer : public UpdatableObject { + + private: + MembersContainer members_; + public: // limited container interface typedef vector MembersContainer; typedef typename MembersContainer::iterator iterator; @@ -39,7 +43,6 @@ template class UpdatableContainer : public Updata members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end()); } - public: //*** Start: UpdatableState implementation ***// virtual void resetImplementation() override { for (TUpdatableObjectPtr &member : members_) @@ -60,9 +63,6 @@ template class UpdatableContainer : public Updata //*** End: UpdatableState implementation ***// virtual ~UpdatableContainer() = default; - - private: - MembersContainer members_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/UpdatableObject.hpp b/AutonomyLib/include/common/UpdatableObject.hpp index 6f219b92..e5f77e9f 100644 --- a/AutonomyLib/include/common/UpdatableObject.hpp +++ b/AutonomyLib/include/common/UpdatableObject.hpp @@ -5,8 +5,8 @@ #define autonomylib_common_UpdatableObject_hpp #include "ClockFactory.hpp" +#include "Common.hpp" #include "StateReporter.hpp" -#include "common/Common.hpp" namespace nervosys { namespace autonomylib { @@ -26,6 +26,18 @@ init->reset calls for base-derived class that would be incorrect. */ class UpdatableObject { + + private: + bool reset_called = false; + bool update_called = false; + bool reset_in_progress = false; + UpdatableObject *parent_ = nullptr; + std::string name_; + + protected: + virtual void resetImplementation() = 0; + virtual void failResetUpdateOrdering(std::string err) { throw std::runtime_error(err); } + public: virtual ~UpdatableObject() = default; @@ -68,17 +80,6 @@ class UpdatableObject { std::string getName() { return name_; } void setName(const std::string &name) { this->name_ = name; } - - protected: - virtual void resetImplementation() = 0; - virtual void failResetUpdateOrdering(std::string err) { throw std::runtime_error(err); } - - private: - bool reset_called = false; - bool update_called = false; - bool reset_in_progress = false; - UpdatableObject *parent_ = nullptr; - std::string name_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/VectorMath.hpp b/AutonomyLib/include/common/VectorMath.hpp index 30f63c78..afed945f 100644 --- a/AutonomyLib/include/common/VectorMath.hpp +++ b/AutonomyLib/include/common/VectorMath.hpp @@ -4,10 +4,10 @@ #ifndef autonomylib_common_VectorMath_hpp #define autonomylib_common_VectorMath_hpp -#include +#include "utils/RandomGenerator.hpp" +#include "utils/Utils.hpp" -#include "common/utils/RandomGenerator.hpp" -#include "common/utils/Utils.hpp" +#include STRICT_MODE_OFF // if not using unaligned types then disable vectorization to avoid alignment issues all over the places @@ -19,6 +19,7 @@ namespace nervosys { namespace autonomylib { template class VectorMathT { + public: // IMPORTANT: make sure fixed size vectorization types have no alignment assumption // https://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html @@ -86,6 +87,12 @@ template class VectorMathT { }; class RandomVectorT { + + private: + RandomGeneratorXT rx_; + RandomGeneratorYT ry_; + RandomGeneratorZT rz_; + public: RandomVectorT() {} RandomVectorT(RealT min_val, RealT max_val) @@ -100,14 +107,15 @@ template class VectorMathT { } Vector3T next() { return Vector3T(rx_.next(), ry_.next(), rz_.next()); } - - private: - RandomGeneratorXT rx_; - RandomGeneratorYT ry_; - RandomGeneratorZT rz_; }; class RandomVectorGaussianT { + + private: + RandomGeneratorGausianXT rx_; + RandomGeneratorGausianYT ry_; + RandomGeneratorGausianZT rz_; + public: RandomVectorGaussianT() {} RandomVectorGaussianT(RealT mean, RealT stddev) : rx_(mean, stddev), ry_(mean, stddev), rz_(mean, stddev) {} @@ -121,11 +129,6 @@ template class VectorMathT { } Vector3T next() { return Vector3T(rx_.next(), ry_.next(), rz_.next()); } - - private: - RandomGeneratorGausianXT rx_; - RandomGeneratorGausianYT ry_; - RandomGeneratorGausianZT rz_; }; public: diff --git a/AutonomyLib/include/common/Waiter.hpp b/AutonomyLib/include/common/Waiter.hpp index f3f3e8d8..7d4526a7 100644 --- a/AutonomyLib/include/common/Waiter.hpp +++ b/AutonomyLib/include/common/Waiter.hpp @@ -4,10 +4,11 @@ #ifndef autonomylib_common_Waiter_hpp #define autonomylib_common_Waiter_hpp -#include "common/CancelToken.hpp" -#include "common/ClockFactory.hpp" -#include "common/Common.hpp" -#include "common/utils/Utils.hpp" +#include "CancelToken.hpp" +#include "ClockFactory.hpp" +#include "Common.hpp" +#include "utils/Utils.hpp" + #include #include @@ -15,6 +16,16 @@ namespace nervosys { namespace autonomylib { class Waiter { + + private: + TTimeDelta sleep_duration_, timeout_sec_; + CancelToken &cancelable_action_; + bool is_complete_; // each waiter should maintain its own complete status + TTimePoint proc_start_; + TTimePoint loop_start_; + + static ClockBase *clock() { return ClockFactory::get(); } + public: Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_sec, CancelToken &cancelable_action) : sleep_duration_(sleep_duration_seconds), timeout_sec_(timeout_sec), cancelable_action_(cancelable_action), @@ -50,16 +61,6 @@ class Waiter { else return clock()->elapsedSince(proc_start_) >= timeout_sec_; } - - private: - TTimeDelta sleep_duration_, timeout_sec_; - CancelToken &cancelable_action_; - bool is_complete_; // each waiter should maintain its own complete status - - TTimePoint proc_start_; - TTimePoint loop_start_; - - static ClockBase *clock() { return ClockFactory::get(); } }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/WorkerThread.hpp b/AutonomyLib/include/common/WorkerThread.hpp index 38f6cd82..d77788f6 100644 --- a/AutonomyLib/include/common/WorkerThread.hpp +++ b/AutonomyLib/include/common/WorkerThread.hpp @@ -6,7 +6,8 @@ #include "CancelToken.hpp" #include "ClockFactory.hpp" //TODO: move this out of common\utils -#include "common/utils/Utils.hpp" +#include "utils/Utils.hpp" + #include #include #include @@ -20,6 +21,10 @@ namespace nervosys { namespace autonomylib { class CancelableAction : public CancelToken { + + private: + std::atomic is_complete_; + protected: virtual void executeAction() = 0; @@ -40,14 +45,13 @@ class CancelableAction : public CancelToken { } bool isComplete() const { return is_complete_; } - - private: - std::atomic is_complete_; }; // This wraps a condition_variable so we can handle the case where we may signal before wait // and implement the semantics that say wait should be a noop in that case. class WorkerThreadSignal { + + private: std::condition_variable cv_; std::mutex mutex_; std::atomic signaled_; @@ -116,6 +120,85 @@ class WorkerThreadSignal { // task before they get canceled, worst case in a tight loop all tasks are starved and // nothing executes. class WorkerThread { + + private: + // this is used to wait until our thread actually gets started + WorkerThreadSignal thread_started_; + // when new item arrived, we signal this so waiting thread can continue + WorkerThreadSignal item_arrived_; + + // thread state + std::shared_ptr pending_item_; + std::mutex mutex_; + std::thread thread_; + // while run() is in progress this is true + std::atomic thread_running_; + // has request to stop this worker thread made? + std::atomic cancel_request_; + + void start() { + // if state == not running + if (!thread_running_) { + + // make sure C++ previous thread is done + { + std::unique_lock lock(mutex_); + if (thread_.joinable()) { + thread_.join(); + } + } + Utils::cleanupThread(thread_); + + // start the thread + cancel_request_ = false; + thread_ = std::thread(&WorkerThread::run, this); + + // wait until thread tells us it has started + thread_started_.wait([this] { return static_cast(cancel_request_); }); + } + } + + void run() { + thread_running_ = true; + + // tell the thread which started this thread that we are on now + { + std::unique_lock lock(mutex_); + thread_started_.signal(); + } + + // until we don't get stopped and have work to do, keep running + while (!cancel_request_ && pending_item_ != nullptr) { + std::shared_ptr pending; + + // get the pending item + { + std::unique_lock lock(mutex_); + pending = pending_item_; + } + + // if pending item is not yet cancelled + if (pending != nullptr && !pending->isCancelled()) { + + // execute pending item + try { + pending->execute(); + } catch (std::exception &e) { + // Utils::DebugBreak(); + Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), + Utils::kLogLevelError); + } + } + + if (!cancel_request_) { + // wait for next item to arrive or thread is stopped + item_arrived_.wait([this] { return static_cast(cancel_request_); }); + } + } + + thread_running_ = false; + } + public: WorkerThread() : thread_running_(false), cancel_request_(false) {} @@ -123,6 +206,7 @@ class WorkerThread { cancel_request_ = true; cancel(); } + void enqueue(std::shared_ptr item) { // cancel previous item { @@ -194,85 +278,6 @@ class WorkerThread { thread_.join(); } } - - private: - void start() { - // if state == not running - if (!thread_running_) { - - // make sure C++ previous thread is done - { - std::unique_lock lock(mutex_); - if (thread_.joinable()) { - thread_.join(); - } - } - Utils::cleanupThread(thread_); - - // start the thread - cancel_request_ = false; - thread_ = std::thread(&WorkerThread::run, this); - - // wait until thread tells us it has started - thread_started_.wait([this] { return static_cast(cancel_request_); }); - } - } - - void run() { - thread_running_ = true; - - // tell the thread which started this thread that we are on now - { - std::unique_lock lock(mutex_); - thread_started_.signal(); - } - - // until we don't get stopped and have work to do, keep running - while (!cancel_request_ && pending_item_ != nullptr) { - std::shared_ptr pending; - - // get the pending item - { - std::unique_lock lock(mutex_); - pending = pending_item_; - } - - // if pending item is not yet cancelled - if (pending != nullptr && !pending->isCancelled()) { - - // execute pending item - try { - pending->execute(); - } catch (std::exception &e) { - // Utils::DebugBreak(); - Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), - Utils::kLogLevelError); - } - } - - if (!cancel_request_) { - // wait for next item to arrive or thread is stopped - item_arrived_.wait([this] { return static_cast(cancel_request_); }); - } - } - - thread_running_ = false; - } - - private: - // this is used to wait until our thread actually gets started - WorkerThreadSignal thread_started_; - // when new item arrived, we signal this so waiting thread can continue - WorkerThreadSignal item_arrived_; - - // thread state - std::shared_ptr pending_item_; - std::mutex mutex_; - std::thread thread_; - // while run() is in progress this is true - std::atomic thread_running_; - // has request to stop this worker thread made? - std::atomic cancel_request_; }; } // namespace autonomylib diff --git a/AutonomyLib/include/common/utils/AsyncTasker.hpp b/AutonomyLib/include/common/utils/AsyncTasker.hpp index 1257d4c2..389fa3c1 100644 --- a/AutonomyLib/include/common/utils/AsyncTasker.hpp +++ b/AutonomyLib/include/common/utils/AsyncTasker.hpp @@ -5,9 +5,15 @@ #define common_utils_AsyncTasker_hpp #include "ctpl_stl.h" + #include class AsyncTasker { + + private: + ctpl::thread_pool threads_; + std::function error_handler_; + public: AsyncTasker(unsigned int thread_count = 4) : threads_(thread_count), error_handler_([](std::exception e) { unused(e); }) {} @@ -41,10 +47,6 @@ class AsyncTasker { }); } } - - private: - ctpl::thread_pool threads_; - std::function error_handler_; }; #endif diff --git a/AutonomyLib/include/common/utils/ColorUtils.hpp b/AutonomyLib/include/common/utils/ColorUtils.hpp index 2acce784..7e7f7127 100644 --- a/AutonomyLib/include/common/utils/ColorUtils.hpp +++ b/AutonomyLib/include/common/utils/ColorUtils.hpp @@ -4,6 +4,7 @@ namespace common_utils { class ColorUtils { + public: static void valToRGB(double val0To1, unsigned char &r, unsigned char &g, diff --git a/AutonomyLib/include/common/utils/EnumFlags.hpp b/AutonomyLib/include/common/utils/EnumFlags.hpp index 8a3dc10c..5cb21ac1 100644 --- a/AutonomyLib/include/common/utils/EnumFlags.hpp +++ b/AutonomyLib/include/common/utils/EnumFlags.hpp @@ -7,6 +7,7 @@ namespace common_utils { template ::type> class EnumFlags { + protected: TUnderlying flags_; diff --git a/AutonomyLib/include/common/utils/FileSystem.hpp b/AutonomyLib/include/common/utils/FileSystem.hpp index c8420926..247c0611 100644 --- a/AutonomyLib/include/common/utils/FileSystem.hpp +++ b/AutonomyLib/include/common/utils/FileSystem.hpp @@ -5,6 +5,7 @@ #define common_utils_FileSystem_hpp #include "Utils.hpp" + #include #include #include @@ -20,7 +21,10 @@ #endif namespace common_utils { + class FileSystem { + + private: typedef unsigned int uint; public: diff --git a/AutonomyLib/include/common/utils/MedianFilter.hpp b/AutonomyLib/include/common/utils/MedianFilter.hpp index 6c459866..a8e2b5bc 100644 --- a/AutonomyLib/include/common/utils/MedianFilter.hpp +++ b/AutonomyLib/include/common/utils/MedianFilter.hpp @@ -11,6 +11,7 @@ namespace common_utils { template class MedianFilter { + private: std::vector buffer_, buffer_copy_; int window_size_, window_size_2x_, window_size_half_; diff --git a/AutonomyLib/include/common/utils/OnlineStats.hpp b/AutonomyLib/include/common/utils/OnlineStats.hpp index faceffae..208fec63 100644 --- a/AutonomyLib/include/common/utils/OnlineStats.hpp +++ b/AutonomyLib/include/common/utils/OnlineStats.hpp @@ -9,6 +9,11 @@ namespace common_utils { class OnlineStats { + + private: + int64_t n; // don't declare as unsigned because we do n-k and if n = 0, we get large number + double m1, m2, m3, m4; + public: OnlineStats() { clear(); } @@ -77,10 +82,6 @@ class OnlineStats { return *this; } - private: - int64_t n; // don't declare as unsigned because we do n-k and if n = 0, we get large number - double m1, m2, m3, m4; - }; // class } // namespace common_utils diff --git a/AutonomyLib/include/common/utils/ProsumerQueue.hpp b/AutonomyLib/include/common/utils/ProsumerQueue.hpp index 7d2665e5..f1c6074d 100644 --- a/AutonomyLib/include/common/utils/ProsumerQueue.hpp +++ b/AutonomyLib/include/common/utils/ProsumerQueue.hpp @@ -13,24 +13,31 @@ namespace common_utils { /* - This queue can support multiple producers consumers, but it should be used carefully - because its a *blocking* queue after all. Do not treat as black box, read the code for - what and how its doing so you know what will happen. There are non-blocking versions available - out there but more often than not they are buggy. This one is simpler and effortlessly cross-platform. - - In multi-consumer scenario, ideally the consumer - thread that does the pop, should also spend time on working on poped item instead of - handing over item to some other thread and going back to another pop right away. If you - do that then other consumer threads might starve and become pointless. Similarly - in multi-producer scenario, the thread doing the push should not immediately come back to - push. So ideally, producer and consumer threads should also perform any time consuming tasks - such as I/O so overall throughput of multi-producer/multi-consumer is maximized. You can tune - the number of thread so queue size doesn't grow out of bound. If you have - only one producer and oner consumer than it might be better idea to do time consuming stuff - such as I/O on sepratae threads so queue doesn't become too large. +This queue can support multiple producers consumers, but it should be used carefully +because its a *blocking* queue after all. Do not treat as black box, read the code for +what and how its doing so you know what will happen. There are non-blocking versions available +out there but more often than not they are buggy. This one is simpler and effortlessly cross-platform. + +In multi-consumer scenario, ideally the consumer +thread that does the pop, should also spend time on working on poped item instead of +handing over item to some other thread and going back to another pop right away. If you +do that then other consumer threads might starve and become pointless. Similarly +in multi-producer scenario, the thread doing the push should not immediately come back to +push. So ideally, producer and consumer threads should also perform any time consuming tasks +such as I/O so overall throughput of multi-producer/multi-consumer is maximized. You can tune +the number of thread so queue size doesn't grow out of bound. If you have +only one producer and oner consumer than it might be better idea to do time consuming stuff +such as I/O on sepratae threads so queue doesn't become too large. */ template class ProsumerQueue { + + private: + std::queue queue_; + std::mutex mutex_; + std::condition_variable cond_; + std::atomic is_done_; + public: ProsumerQueue() { is_done_ = false; } @@ -81,17 +88,13 @@ template class ProsumerQueue { // is_done_ flag is just convinience flag for external use // its not used by this class bool getIsDone() { return is_done_; } + void setIsDone(bool val) { is_done_ = val; } // non-copiable ProsumerQueue(const ProsumerQueue &) = delete; - ProsumerQueue &operator=(const ProsumerQueue &) = delete; - private: - std::queue queue_; - std::mutex mutex_; - std::condition_variable cond_; - std::atomic is_done_; + ProsumerQueue &operator=(const ProsumerQueue &) = delete; }; } // namespace common_utils #endif diff --git a/AutonomyLib/include/common/utils/RandomGenerator.hpp b/AutonomyLib/include/common/utils/RandomGenerator.hpp index ffb0bfb8..246b5dd5 100644 --- a/AutonomyLib/include/common/utils/RandomGenerator.hpp +++ b/AutonomyLib/include/common/utils/RandomGenerator.hpp @@ -9,6 +9,11 @@ namespace common_utils { template class RandomGenerator { + + private: + TDistribution dist_; + std::mt19937 rand_; + public: // for uniform distribution supply min and max (inclusive) // for gaussian distribution supply mean and sigma @@ -22,10 +27,6 @@ template clas rand_.seed(Seed); dist_.reset(); } - - private: - TDistribution dist_; - std::mt19937 rand_; }; typedef RandomGenerator> RandomGeneratorD; diff --git a/AutonomyLib/include/common/utils/ScheduledExecutor.hpp b/AutonomyLib/include/common/utils/ScheduledExecutor.hpp index c23eb602..f824a8b5 100644 --- a/AutonomyLib/include/common/utils/ScheduledExecutor.hpp +++ b/AutonomyLib/include/common/utils/ScheduledExecutor.hpp @@ -15,100 +15,33 @@ namespace common_utils { class ScheduledExecutor { - public: - ScheduledExecutor() {} - - ScheduledExecutor(const std::function &callback, uint64_t period_nanos) { - initialize(callback, period_nanos); - } - - ~ScheduledExecutor() { stop(); } - - void initialize(const std::function &callback, uint64_t period_nanos) { - callback_ = callback; - period_nanos_ = period_nanos; - started_ = false; - frame_countdown_enabled_ = false; - } - - void start() { - started_ = true; - is_first_period_ = true; - - initializePauseState(); - - sleep_time_avg_ = 0; - Utils::cleanupThread(th_); - th_ = std::thread(&ScheduledExecutor::executorLoop, this); - } - - void pause(bool is_paused) { - paused_ = is_paused; - pause_period_start_ = 0; // cancel any pause period. - } - - bool isPaused() const { return paused_; } - - void pauseForTime(double seconds) { - pause_period_start_ = nanos(); - pause_period_ = static_cast(1E9 * seconds); - paused_ = true; - } - - void continueForTime(double seconds) { - pause_period_start_ = nanos(); - pause_period_ = static_cast(1E9 * seconds); - paused_ = false; - } - - void continueForFrames(uint32_t frames) { - pause_period_start_ = 0; // cancel any pause period. - frame_countdown_enabled_ = true; - targetFrameNumber_ = frames + currentFrameNumber_; - paused_ = false; - } - void setFrameNumber(uint32_t frameNumber) { currentFrameNumber_ = frameNumber; } - - void stop() { - if (started_) { - started_ = false; - initializePauseState(); - - try { - if (th_.joinable()) { - th_.join(); - } - } catch (const std::system_error & /* e */) { - } - } - } - - bool isRunning() const { return started_ && !paused_; } - - double getSleepTimeAvg() const { - // TODO: make this function thread safe by using atomic types - // right now this is not implemented for performance and that - // return of this function is purely informational/debugging purposes - return sleep_time_avg_; - } + private: + typedef std::chrono::high_resolution_clock clock; + typedef uint64_t TTimePoint; + typedef uint64_t TTimeDelta; + template using duration = std::chrono::duration; - void lock() { mutex_.lock(); } - void unlock() { mutex_.unlock(); } + uint64_t period_nanos_; + std::thread th_; + std::function callback_; + bool is_first_period_; + std::atomic_bool started_; + std::atomic_bool paused_; + std::atomic pause_period_; + std::atomic pause_period_start_; + uint32_t currentFrameNumber_; + uint32_t targetFrameNumber_; + std::atomic_bool frame_countdown_enabled_; + double sleep_time_avg_; + std::mutex mutex_; - private: void initializePauseState() { paused_ = false; pause_period_start_ = 0; pause_period_ = 0; } - private: - typedef std::chrono::high_resolution_clock clock; - typedef uint64_t TTimePoint; - typedef uint64_t TTimeDelta; - template using duration = std::chrono::duration; - static TTimePoint nanos() { return clock::now().time_since_epoch().count(); } static void sleep_for(TTimePoint delay_nanos) { @@ -180,22 +113,86 @@ class ScheduledExecutor { } } - private: - uint64_t period_nanos_; - std::thread th_; - std::function callback_; - bool is_first_period_; - std::atomic_bool started_; - std::atomic_bool paused_; - std::atomic pause_period_; - std::atomic pause_period_start_; - uint32_t currentFrameNumber_; - uint32_t targetFrameNumber_; - std::atomic_bool frame_countdown_enabled_; + public: + ScheduledExecutor() {} - double sleep_time_avg_; + ScheduledExecutor(const std::function &callback, uint64_t period_nanos) { + initialize(callback, period_nanos); + } - std::mutex mutex_; + ~ScheduledExecutor() { stop(); } + + void initialize(const std::function &callback, uint64_t period_nanos) { + callback_ = callback; + period_nanos_ = period_nanos; + started_ = false; + frame_countdown_enabled_ = false; + } + + void start() { + started_ = true; + is_first_period_ = true; + + initializePauseState(); + + sleep_time_avg_ = 0; + Utils::cleanupThread(th_); + th_ = std::thread(&ScheduledExecutor::executorLoop, this); + } + + void pause(bool is_paused) { + paused_ = is_paused; + pause_period_start_ = 0; // cancel any pause period. + } + + bool isPaused() const { return paused_; } + + void pauseForTime(double seconds) { + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); + paused_ = true; + } + + void continueForTime(double seconds) { + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); + paused_ = false; + } + + void continueForFrames(uint32_t frames) { + pause_period_start_ = 0; // cancel any pause period. + frame_countdown_enabled_ = true; + targetFrameNumber_ = frames + currentFrameNumber_; + paused_ = false; + } + + void setFrameNumber(uint32_t frameNumber) { currentFrameNumber_ = frameNumber; } + + void stop() { + if (started_) { + started_ = false; + initializePauseState(); + + try { + if (th_.joinable()) { + th_.join(); + } + } catch (const std::system_error & /* e */) { + } + } + } + + bool isRunning() const { return started_ && !paused_; } + + double getSleepTimeAvg() const { + // TODO: make this function thread safe by using atomic types + // right now this is not implemented for performance and that + // return of this function is purely informational/debugging purposes + return sleep_time_avg_; + } + + void lock() { mutex_.lock(); } + void unlock() { mutex_.unlock(); } }; } // namespace common_utils #endif diff --git a/AutonomyLib/include/common/utils/Signal.hpp b/AutonomyLib/include/common/utils/Signal.hpp index 7cb26e5d..171fc74e 100644 --- a/AutonomyLib/include/common/utils/Signal.hpp +++ b/AutonomyLib/include/common/utils/Signal.hpp @@ -37,6 +37,10 @@ namespace common_utils { template class Signal { + private: + mutable std::map> slots_; + mutable int current_id_; + public: Signal() : current_id_(0) {} @@ -74,10 +78,6 @@ template class Signal { (it++)->second(p...); } } - - private: - mutable std::map> slots_; - mutable int current_id_; }; } // namespace common_utils diff --git a/AutonomyLib/include/common/utils/SmoothingFilter.hpp b/AutonomyLib/include/common/utils/SmoothingFilter.hpp index 028f2704..cf47cafa 100644 --- a/AutonomyLib/include/common/utils/SmoothingFilter.hpp +++ b/AutonomyLib/include/common/utils/SmoothingFilter.hpp @@ -13,6 +13,7 @@ namespace common_utils { // SmoothingFilter is a filter that removes the outliers that fall outside a given percentage from the min/max // range of numbers over a given window size. template class SmoothingFilter { + private: std::vector buffer_; int window_size_; diff --git a/AutonomyLib/include/common/utils/Timer.hpp b/AutonomyLib/include/common/utils/Timer.hpp index 6bac7f0e..893b5b11 100644 --- a/AutonomyLib/include/common/utils/Timer.hpp +++ b/AutonomyLib/include/common/utils/Timer.hpp @@ -2,45 +2,55 @@ #define common_utils_Timer_hpp #include "Utils.hpp" + #include namespace common_utils { class Timer { + + private: + int64_t start_; + int64_t end_; + bool started_; + + int64_t now() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + } + + int64_t end() { + if (started_) { + // not stopped yet, so return "elapsed time so far". + end_ = now(); + } + return end_; + } + public: Timer() { started_ = false; } + void start() { started_ = true; start_ = now(); } + void stop() { started_ = false; end_ = now(); } + double seconds() { auto diff = static_cast(end() - start_); return diff / 1000000.0; } + double milliseconds() { return static_cast(end() - start_) / 1000.0; } + double microseconds() { return static_cast(end() - start_); } - bool started() { return started_; } - private: - int64_t now() { - return std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count(); - } - int64_t end() { - if (started_) { - // not stopped yet, so return "elapsed time so far". - end_ = now(); - } - return end_; - } - int64_t start_; - int64_t end_; - bool started_; + bool started() { return started_; } }; } // namespace common_utils #endif \ No newline at end of file diff --git a/AutonomyLib/include/common/utils/UniqueValueMap.hpp b/AutonomyLib/include/common/utils/UniqueValueMap.hpp index 8241e7c4..2602f07b 100644 --- a/AutonomyLib/include/common/utils/UniqueValueMap.hpp +++ b/AutonomyLib/include/common/utils/UniqueValueMap.hpp @@ -2,6 +2,7 @@ #define common_utils_UniqueValueMap_hpp #include "Utils.hpp" + #include #include #include @@ -11,11 +12,17 @@ namespace common_utils { // This class allows to maintain unique set of values while // still allowing key to value maps template class UniqueValueMap { + + private: + std::map map_; + std::set vals_; + public: void insert(const TKey &key, const TVal &val) { map_.insert(key, val); vals_.insert(val); } + void insert_or_assign(const TKey &key, const TVal &val) { map_[key] = val; vals_.insert(val); @@ -53,10 +60,6 @@ template class UniqueValueMap { } // TODO: add erase methods - - private: - std::map map_; - std::set vals_; }; } // namespace common_utils #endif diff --git a/AutonomyLib/include/common/utils/Utils.hpp b/AutonomyLib/include/common/utils/Utils.hpp index ec073da2..6049c3d2 100644 --- a/AutonomyLib/include/common/utils/Utils.hpp +++ b/AutonomyLib/include/common/utils/Utils.hpp @@ -6,6 +6,7 @@ #include "StrictMode.hpp" #include "type_utils.hpp" + #include #include #include @@ -49,10 +50,10 @@ #define EARTH_RADIUS (6378137.0f) /* - This file is collection of routines that can be included in ANY project just - by dropping in common_utils.hpp. Therefore there should not be any dependency - in the code below other than STL. The code should be able to compilable on - all major platforms. +This file is collection of routines that can be included in ANY project just +by dropping in common_utils.hpp. Therefore there should not be any dependency +in the code below other than STL. The code should be able to compilable on +all major platforms. */ #ifndef _MSC_VER @@ -74,6 +75,7 @@ template inline void unused(T const &result) { static_cast(resul namespace common_utils { class Utils { + private: typedef std::chrono::system_clock system_clock; typedef std::chrono::steady_clock steady_clock; @@ -147,10 +149,12 @@ class Utils { static constexpr int kLogLevelInfo = 0; static constexpr int kLogLevelWarn = -1; static constexpr int kLogLevelError = -2; + static void log(std::string message, int level = kLogLevelInfo) { if (level >= getSetMinLogLevel()) getSetLogger()->log(level, message); } + static int getSetMinLogLevel(bool set_or_get = false, int set_min_log_level = std::numeric_limits::min()) { static int min_log_level = std::numeric_limits::min(); if (set_or_get) @@ -179,6 +183,7 @@ class Utils { const string &suffix = ")") { return printRange(std::begin(range), std::end(range), delim, prefix, suffix); } + template static const string printRange(Iterator start, Iterator last, const string &delim = ", ", const string &prefix = "(", const string &suffix = ")") { @@ -228,9 +233,7 @@ class Utils { #else vsnprintf_s(buf.get(), size, _TRUNCATE, format, args); #endif - va_end(args); - return string(buf.get()); } @@ -251,6 +254,7 @@ class Utils { return ""; return str.substr(i, j - i + 1); } + static std::vector split(const string &s, const char *splitChars, int numSplitChars) { auto start = s.begin(); std::vector result; @@ -377,6 +381,7 @@ class Utils { std::ofstream file(file_name, std::ios::binary); file.write(data, size); } + template static typename std::enable_if::value, void>::type append(Container &to, const Container &from) { @@ -384,6 +389,7 @@ class Utils { using std::end; to.insert(end(to), begin(from), end(from)); } + template static typename std::enable_if::value, void>::type copy(const Container &from, Container &to) { @@ -391,6 +397,7 @@ class Utils { using std::end; std::copy(begin(from), end(from), begin(to)); } + template static void copy(const T *from, T *to, uint count) { std::copy(from, from + count, to); } static const char *to_string(time_point t) { @@ -432,6 +439,7 @@ class Utils { else return string(); } + static string getLogFileTimeStamp() { return to_string(now(), "%Y%m%d%H%M%S"); } static int to_integer(std::string s) { return atoi(s.c_str()); } static string getEnv(const string &var) { @@ -451,6 +459,7 @@ class Utils { using Clock = std::chrono::system_clock; // high res clock has epoch since boot instead of since 1970 for VC++ return std::chrono::duration((t != nullptr ? *t : Clock::now()).time_since_epoch()).count(); } + static uint64_t getTimeSinceEpochNanos(std::chrono::system_clock::time_point *t = nullptr) { using Clock = std::chrono::system_clock; // high res clock has epoch since boot instead of since 1970 for VC++ return std::chrono::duration_cast( @@ -513,6 +522,7 @@ class Utils { return false; } + template static bool isDefinitelyGreaterThan(TReal a, TReal b, TReal tolerance = epsilon()) { TReal diff = a - b; @@ -548,6 +558,7 @@ class Utils { template static constexpr typename std::underlying_type::type toNumeric(E e) { return static_cast::type>(e); } + template static constexpr E toEnum(typename std::underlying_type::type u) { return static_cast(u); } diff --git a/AutonomyLib/include/common/utils/json.hpp b/AutonomyLib/include/common/utils/json.hpp index cb27e058..4ff025e2 100644 --- a/AutonomyLib/include/common/utils/json.hpp +++ b/AutonomyLib/include/common/utils/json.hpp @@ -1,31 +1,10 @@ -/* - __ _____ _____ _____ - __| | __| | | | JSON for Modern C++ -| | |__ | | | | | | version 3.10.5 -|_____|_____|_____|_|___| https://github.com/nlohmann/json - -Licensed under the MIT License . -SPDX-License-Identifier: MIT -Copyright (c) 2013-2022 Niels Lohmann . - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT /****************************************************************************\ * Note on documentation: The source files contain links to the online * @@ -33,16 +12,13 @@ SOFTWARE. * contains the most recent documentation and should also be applicable to * * previous versions; documentation for deprecated functions is not * * removed, but marked deprecated. See "Generate documentation" section in * - * file doc/README.md. * + * file docs/README.md. * \****************************************************************************/ +// clang-format off #ifndef INCLUDE_NLOHMANN_JSON_HPP_ #define INCLUDE_NLOHMANN_JSON_HPP_ -#define NLOHMANN_JSON_VERSION_MAJOR 3 -#define NLOHMANN_JSON_VERSION_MINOR 10 -#define NLOHMANN_JSON_VERSION_PATCH 5 - #include // all_of, find, for_each #include // nullptr_t, ptrdiff_t, size_t #include // hash, less @@ -52,18 +28,134 @@ SOFTWARE. #endif // JSON_NO_IO #include // random_access_iterator_tag #include // unique_ptr -#include // accumulate #include // string, stoi, to_string #include // declval, forward, move, pair, swap #include // vector // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + -#include #include +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + + + +// This file contains all macro definitions affecting or depending on the ABI + +#ifndef JSON_SKIP_LIBRARY_VERSION_CHECK + #if defined(NLOHMANN_JSON_VERSION_MAJOR) && defined(NLOHMANN_JSON_VERSION_MINOR) && defined(NLOHMANN_JSON_VERSION_PATCH) + #if NLOHMANN_JSON_VERSION_MAJOR != 3 || NLOHMANN_JSON_VERSION_MINOR != 11 || NLOHMANN_JSON_VERSION_PATCH != 3 + #warning "Already included a different version of the library!" + #endif + #endif +#endif + +#define NLOHMANN_JSON_VERSION_MAJOR 3 // NOLINT(modernize-macro-to-enum) +#define NLOHMANN_JSON_VERSION_MINOR 11 // NOLINT(modernize-macro-to-enum) +#define NLOHMANN_JSON_VERSION_PATCH 3 // NOLINT(modernize-macro-to-enum) + +#ifndef JSON_DIAGNOSTICS + #define JSON_DIAGNOSTICS 0 +#endif + +#ifndef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON + #define JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON 0 +#endif + +#if JSON_DIAGNOSTICS + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS _diag +#else + #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS +#endif + +#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON + #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON _ldvcmp +#else + #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_NO_VERSION + #define NLOHMANN_JSON_NAMESPACE_NO_VERSION 0 +#endif + +// Construct the namespace ABI tags component +#define NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b) json_abi ## a ## b +#define NLOHMANN_JSON_ABI_TAGS_CONCAT(a, b) \ + NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b) + +#define NLOHMANN_JSON_ABI_TAGS \ + NLOHMANN_JSON_ABI_TAGS_CONCAT( \ + NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS, \ + NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON) + +// Construct the namespace version component +#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) \ + _v ## major ## _ ## minor ## _ ## patch +#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(major, minor, patch) \ + NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) + +#if NLOHMANN_JSON_NAMESPACE_NO_VERSION +#define NLOHMANN_JSON_NAMESPACE_VERSION +#else +#define NLOHMANN_JSON_NAMESPACE_VERSION \ + NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(NLOHMANN_JSON_VERSION_MAJOR, \ + NLOHMANN_JSON_VERSION_MINOR, \ + NLOHMANN_JSON_VERSION_PATCH) +#endif + +// Combine namespace components +#define NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) a ## b +#define NLOHMANN_JSON_NAMESPACE_CONCAT(a, b) \ + NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) + +#ifndef NLOHMANN_JSON_NAMESPACE +#define NLOHMANN_JSON_NAMESPACE \ + nlohmann::NLOHMANN_JSON_NAMESPACE_CONCAT( \ + NLOHMANN_JSON_ABI_TAGS, \ + NLOHMANN_JSON_NAMESPACE_VERSION) +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_BEGIN +#define NLOHMANN_JSON_NAMESPACE_BEGIN \ + namespace nlohmann \ + { \ + inline namespace NLOHMANN_JSON_NAMESPACE_CONCAT( \ + NLOHMANN_JSON_ABI_TAGS, \ + NLOHMANN_JSON_NAMESPACE_VERSION) \ + { +#endif + +#ifndef NLOHMANN_JSON_NAMESPACE_END +#define NLOHMANN_JSON_NAMESPACE_END \ + } /* namespace (inline namespace) NOLINT(readability/namespace) */ \ + } // namespace nlohmann +#endif + // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #include // transform @@ -79,14 +171,34 @@ SOFTWARE. #include // valarray // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + +#include // nullptr_t #include // exception +#if JSON_DIAGNOSTICS + #include // accumulate +#endif #include // runtime_error #include // to_string #include // vector // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #include // array @@ -94,102 +206,130 @@ SOFTWARE. #include // uint8_t #include // string -namespace nlohmann -{ +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + + + +#include // declval, pair +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + + + +#include + +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + + + +// #include + + +NLOHMANN_JSON_NAMESPACE_BEGIN namespace detail { -/////////////////////////// -// JSON type enumeration // -/////////////////////////// -/*! -@brief the JSON type enumeration +template struct make_void +{ + using type = void; +}; +template using void_t = typename make_void::type; -This enumeration collects the different JSON types. It is internally used to -distinguish the stored values, and the functions @ref basic_json::is_null(), -@ref basic_json::is_object(), @ref basic_json::is_array(), -@ref basic_json::is_string(), @ref basic_json::is_boolean(), -@ref basic_json::is_number() (with @ref basic_json::is_number_integer(), -@ref basic_json::is_number_unsigned(), and @ref basic_json::is_number_float()), -@ref basic_json::is_discarded(), @ref basic_json::is_primitive(), and -@ref basic_json::is_structured() rely on it. +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END -@note There are three enumeration entries (number_integer, number_unsigned, and -number_float), because the library distinguishes these three types for numbers: -@ref basic_json::number_unsigned_t is used for unsigned integers, -@ref basic_json::number_integer_t is used for signed integers, and -@ref basic_json::number_float_t is used for floating-point numbers or to -approximate integers which do not fit in the limits of their respective type. -@sa see @ref basic_json::basic_json(const value_t value_type) -- create a JSON -value with the default value for a given type +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ -@since version 1.0.0 -*/ -enum class value_t : std::uint8_t +// https://en.cppreference.com/w/cpp/experimental/is_detected +struct nonesuch { - null, ///< null value - object, ///< object (unordered set of name/value pairs) - array, ///< array (ordered collection of values) - string, ///< string value - boolean, ///< boolean value - number_integer, ///< number value (signed integer) - number_unsigned, ///< number value (unsigned integer) - number_float, ///< number value (floating-point) - binary, ///< binary array (ordered collection of bytes) - discarded ///< discarded by the parser callback function + nonesuch() = delete; + ~nonesuch() = delete; + nonesuch(nonesuch const&) = delete; + nonesuch(nonesuch const&&) = delete; + void operator=(nonesuch const&) = delete; + void operator=(nonesuch&&) = delete; }; -/*! -@brief comparison operator for JSON types - -Returns an ordering that is similar to Python: -- order: null < boolean < number < object < array < string < binary -- furthermore, each type is not smaller than itself -- discarded values are not comparable -- binary is represented as a b"" string in python and directly comparable to a - string; however, making a binary array directly comparable with a string would - be surprising behavior in a JSON file. +template class Op, + class... Args> +struct detector +{ + using value_t = std::false_type; + using type = Default; +}; -@since version 1.0.0 -*/ -inline bool operator<(const value_t lhs, const value_t rhs) noexcept +template class Op, class... Args> +struct detector>, Op, Args...> { - static constexpr std::array order = {{ - 0 /* null */, 3 /* object */, 4 /* array */, 5 /* string */, - 1 /* boolean */, 2 /* integer */, 2 /* unsigned */, 2 /* float */, - 6 /* binary */ - } - }; + using value_t = std::true_type; + using type = Op; +}; - const auto l_index = static_cast(lhs); - const auto r_index = static_cast(rhs); - return l_index < order.size() && r_index < order.size() && order[l_index] < order[r_index]; -} -} // namespace detail -} // namespace nlohmann +template class Op, class... Args> +using is_detected = typename detector::value_t; -// #include +template class Op, class... Args> +struct is_detected_lazy : is_detected { }; +template class Op, class... Args> +using detected_t = typename detector::type; -#include -// #include +template class Op, class... Args> +using detected_or = detector; + +template class Op, class... Args> +using detected_or_t = typename detected_or::type; +template class Op, class... Args> +using is_detected_exact = std::is_same>; + +template class Op, class... Args> +using is_detected_convertible = + std::is_convertible, To>; + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END -#include // declval, pair // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-FileCopyrightText: 2016-2021 Evan Nemerson +// SPDX-License-Identifier: MIT + /* Hedley - https://nemequ.github.io/hedley * Created by Evan Nemerson - * - * To the extent possible under law, the author(s) have dedicated all - * copyright and related and neighboring rights to this software to - * the public domain worldwide. This software is distributed without - * any warranty. - * - * For details, see . - * SPDX-License-Identifier: CC0-1.0 */ #if !defined(JSON_HEDLEY_VERSION) || (JSON_HEDLEY_VERSION < 15) @@ -2223,116 +2363,48 @@ JSON_HEDLEY_DIAGNOSTIC_POP #endif /* !defined(JSON_HEDLEY_VERSION) || (JSON_HEDLEY_VERSION < X) */ -// #include - -#include - -// #include +// This file contains all internal macro definitions (except those affecting ABI) +// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them +// #include -namespace nlohmann -{ -namespace detail -{ -template struct make_void -{ - using type = void; -}; -template using void_t = typename make_void::type; -} // namespace detail -} // namespace nlohmann +// exclude unsupported compilers +#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) + #if defined(__clang__) + #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 + #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) + #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 + #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #endif +#endif -// https://en.cppreference.com/w/cpp/experimental/is_detected -namespace nlohmann -{ -namespace detail -{ -struct nonesuch -{ - nonesuch() = delete; - ~nonesuch() = delete; - nonesuch(nonesuch const&) = delete; - nonesuch(nonesuch const&&) = delete; - void operator=(nonesuch const&) = delete; - void operator=(nonesuch&&) = delete; -}; +// C++ language standard detection +// if the user manually specified the used c++ version this is skipped +#if !defined(JSON_HAS_CPP_20) && !defined(JSON_HAS_CPP_17) && !defined(JSON_HAS_CPP_14) && !defined(JSON_HAS_CPP_11) + #if (defined(__cplusplus) && __cplusplus >= 202002L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L) + #define JSON_HAS_CPP_20 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 + #elif (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 + #elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) + #define JSON_HAS_CPP_14 + #endif + // the cpp 11 flag is always specified because it is the minimal required version + #define JSON_HAS_CPP_11 +#endif -template class Op, - class... Args> -struct detector -{ - using value_t = std::false_type; - using type = Default; -}; - -template class Op, class... Args> -struct detector>, Op, Args...> -{ - using value_t = std::true_type; - using type = Op; -}; - -template class Op, class... Args> -using is_detected = typename detector::value_t; - -template class Op, class... Args> -struct is_detected_lazy : is_detected { }; - -template class Op, class... Args> -using detected_t = typename detector::type; - -template class Op, class... Args> -using detected_or = detector; - -template class Op, class... Args> -using detected_or_t = typename detected_or::type; - -template class Op, class... Args> -using is_detected_exact = std::is_same>; - -template class Op, class... Args> -using is_detected_convertible = - std::is_convertible, To>; -} // namespace detail -} // namespace nlohmann - - -// This file contains all internal macro definitions -// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them - -// exclude unsupported compilers -#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) - #if defined(__clang__) - #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 - #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" - #endif - #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) - #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 - #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" - #endif - #endif -#endif - -// C++ language standard detection -// if the user manually specified the used c++ version this is skipped -#if !defined(JSON_HAS_CPP_20) && !defined(JSON_HAS_CPP_17) && !defined(JSON_HAS_CPP_14) && !defined(JSON_HAS_CPP_11) - #if (defined(__cplusplus) && __cplusplus >= 202002L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L) - #define JSON_HAS_CPP_20 - #define JSON_HAS_CPP_17 - #define JSON_HAS_CPP_14 - #elif (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 - #define JSON_HAS_CPP_17 - #define JSON_HAS_CPP_14 - #elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) - #define JSON_HAS_CPP_14 - #endif - // the cpp 11 flag is always specified because it is the minimal required version - #define JSON_HAS_CPP_11 -#endif +#ifdef __has_include + #if __has_include() + #include + #endif +#endif #if !defined(JSON_HAS_FILESYSTEM) && !defined(JSON_HAS_EXPERIMENTAL_FILESYSTEM) #ifdef JSON_HAS_CPP_17 @@ -2367,7 +2439,7 @@ using is_detected_convertible = #endif // no filesystem support before MSVC 19.14: https://en.cppreference.com/w/cpp/compiler_support - #if defined(_MSC_VER) && _MSC_VER < 1940 + #if defined(_MSC_VER) && _MSC_VER < 1914 #undef JSON_HAS_FILESYSTEM #undef JSON_HAS_EXPERIMENTAL_FILESYSTEM #endif @@ -2394,6 +2466,46 @@ using is_detected_convertible = #define JSON_HAS_FILESYSTEM 0 #endif +#ifndef JSON_HAS_THREE_WAY_COMPARISON + #if defined(__cpp_impl_three_way_comparison) && __cpp_impl_three_way_comparison >= 201907L \ + && defined(__cpp_lib_three_way_comparison) && __cpp_lib_three_way_comparison >= 201907L + #define JSON_HAS_THREE_WAY_COMPARISON 1 + #else + #define JSON_HAS_THREE_WAY_COMPARISON 0 + #endif +#endif + +#ifndef JSON_HAS_RANGES + // ranges header shipping in GCC 11.1.0 (released 2021-04-27) has syntax error + #if defined(__GLIBCXX__) && __GLIBCXX__ == 20210427 + #define JSON_HAS_RANGES 0 + #elif defined(__cpp_lib_ranges) + #define JSON_HAS_RANGES 1 + #else + #define JSON_HAS_RANGES 0 + #endif +#endif + +#ifndef JSON_HAS_STATIC_RTTI + #if !defined(_HAS_STATIC_RTTI) || _HAS_STATIC_RTTI != 0 + #define JSON_HAS_STATIC_RTTI 1 + #else + #define JSON_HAS_STATIC_RTTI 0 + #endif +#endif + +#ifdef JSON_HAS_CPP_17 + #define JSON_INLINE_VARIABLE inline +#else + #define JSON_INLINE_VARIABLE +#endif + +#if JSON_HEDLEY_HAS_ATTRIBUTE(no_unique_address) + #define JSON_NO_UNIQUE_ADDRESS [[no_unique_address]] +#else + #define JSON_NO_UNIQUE_ADDRESS +#endif + // disable documentation warnings on clang #if defined(__clang__) #pragma clang diagnostic push @@ -2489,12 +2601,13 @@ using is_detected_convertible = class NumberUnsignedType, class NumberFloatType, \ template class AllocatorType, \ template class JSONSerializer, \ - class BinaryType> + class BinaryType, \ + class CustomBaseClass> #define NLOHMANN_BASIC_JSON_TPL \ basic_json + AllocatorType, JSONSerializer, BinaryType, CustomBaseClass> // Macros to simplify conversion from/to types @@ -2631,6 +2744,7 @@ using is_detected_convertible = #define NLOHMANN_JSON_TO(v1) nlohmann_json_j[#v1] = nlohmann_json_t.v1; #define NLOHMANN_JSON_FROM(v1) nlohmann_json_j.at(#v1).get_to(nlohmann_json_t.v1); +#define NLOHMANN_JSON_FROM_WITH_DEFAULT(v1) nlohmann_json_t.v1 = nlohmann_json_j.value(#v1, nlohmann_json_default_obj.v1); /*! @brief macro @@ -2641,6 +2755,13 @@ using is_detected_convertible = friend void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ friend void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } +#define NLOHMANN_DEFINE_TYPE_INTRUSIVE_WITH_DEFAULT(Type, ...) \ + friend void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + friend void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } + +#define NLOHMANN_DEFINE_TYPE_INTRUSIVE_ONLY_SERIALIZE(Type, ...) \ + friend void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + /*! @brief macro @def NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE @@ -2650,6 +2771,12 @@ using is_detected_convertible = inline void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ inline void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM, __VA_ARGS__)) } +#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_ONLY_SERIALIZE(Type, ...) \ + inline void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } + +#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE_WITH_DEFAULT(Type, ...) \ + inline void to_json(nlohmann::json& nlohmann_json_j, const Type& nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \ + inline void from_json(const nlohmann::json& nlohmann_json_j, Type& nlohmann_json_t) { const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) } // inspired from https://stackoverflow.com/a/26745591 // allows to call any std function as if (e.g. with begin): @@ -2699,13 +2826,132 @@ using is_detected_convertible = #define JSON_EXPLICIT explicit #endif -#ifndef JSON_DIAGNOSTICS - #define JSON_DIAGNOSTICS 0 +#ifndef JSON_DISABLE_ENUM_SERIALIZATION + #define JSON_DISABLE_ENUM_SERIALIZATION 0 +#endif + +#ifndef JSON_USE_GLOBAL_UDLS + #define JSON_USE_GLOBAL_UDLS 1 +#endif + +#if JSON_HAS_THREE_WAY_COMPARISON + #include // partial_ordering +#endif + +NLOHMANN_JSON_NAMESPACE_BEGIN +namespace detail +{ + +/////////////////////////// +// JSON type enumeration // +/////////////////////////// + +/*! +@brief the JSON type enumeration + +This enumeration collects the different JSON types. It is internally used to +distinguish the stored values, and the functions @ref basic_json::is_null(), +@ref basic_json::is_object(), @ref basic_json::is_array(), +@ref basic_json::is_string(), @ref basic_json::is_boolean(), +@ref basic_json::is_number() (with @ref basic_json::is_number_integer(), +@ref basic_json::is_number_unsigned(), and @ref basic_json::is_number_float()), +@ref basic_json::is_discarded(), @ref basic_json::is_primitive(), and +@ref basic_json::is_structured() rely on it. + +@note There are three enumeration entries (number_integer, number_unsigned, and +number_float), because the library distinguishes these three types for numbers: +@ref basic_json::number_unsigned_t is used for unsigned integers, +@ref basic_json::number_integer_t is used for signed integers, and +@ref basic_json::number_float_t is used for floating-point numbers or to +approximate integers which do not fit in the limits of their respective type. + +@sa see @ref basic_json::basic_json(const value_t value_type) -- create a JSON +value with the default value for a given type + +@since version 1.0.0 +*/ +enum class value_t : std::uint8_t +{ + null, ///< null value + object, ///< object (unordered set of name/value pairs) + array, ///< array (ordered collection of values) + string, ///< string value + boolean, ///< boolean value + number_integer, ///< number value (signed integer) + number_unsigned, ///< number value (unsigned integer) + number_float, ///< number value (floating-point) + binary, ///< binary array (ordered collection of bytes) + discarded ///< discarded by the parser callback function +}; + +/*! +@brief comparison operator for JSON types + +Returns an ordering that is similar to Python: +- order: null < boolean < number < object < array < string < binary +- furthermore, each type is not smaller than itself +- discarded values are not comparable +- binary is represented as a b"" string in python and directly comparable to a + string; however, making a binary array directly comparable with a string would + be surprising behavior in a JSON file. + +@since version 1.0.0 +*/ +#if JSON_HAS_THREE_WAY_COMPARISON + inline std::partial_ordering operator<=>(const value_t lhs, const value_t rhs) noexcept // *NOPAD* +#else + inline bool operator<(const value_t lhs, const value_t rhs) noexcept #endif +{ + static constexpr std::array order = {{ + 0 /* null */, 3 /* object */, 4 /* array */, 5 /* string */, + 1 /* boolean */, 2 /* integer */, 2 /* unsigned */, 2 /* float */, + 6 /* binary */ + } + }; + const auto l_index = static_cast(lhs); + const auto r_index = static_cast(rhs); +#if JSON_HAS_THREE_WAY_COMPARISON + if (l_index < order.size() && r_index < order.size()) + { + return order[l_index] <=> order[r_index]; // *NOPAD* + } + return std::partial_ordering::unordered; +#else + return l_index < order.size() && r_index < order.size() && order[l_index] < order[r_index]; +#endif +} -namespace nlohmann +// GCC selects the built-in operator< over an operator rewritten from +// a user-defined spaceship operator +// Clang, MSVC, and ICC select the rewritten candidate +// (see GCC bug https://gcc.gnu.org/bugzilla/show_bug.cgi?id=105200) +#if JSON_HAS_THREE_WAY_COMPARISON && defined(__GNUC__) +inline bool operator<(const value_t lhs, const value_t rhs) noexcept { + return std::is_lt(lhs <=> rhs); // *NOPAD* +} +#endif + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END + +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + + + +// #include + + +NLOHMANN_JSON_NAMESPACE_BEGIN namespace detail { @@ -2722,12 +2968,13 @@ enforced with an assertion.** @since version 2.0.0 */ -inline void replace_substring(std::string& s, const std::string& f, - const std::string& t) +template +inline void replace_substring(StringType& s, const StringType& f, + const StringType& t) { JSON_ASSERT(!f.empty()); for (auto pos = s.find(f); // find first occurrence of f - pos != std::string::npos; // make sure f was found + pos != StringType::npos; // make sure f was found s.replace(pos, f.size(), t), // replace with t, and pos = s.find(f, pos + t.size())) // find next occurrence of f {} @@ -2740,10 +2987,11 @@ inline void replace_substring(std::string& s, const std::string& f, * * Note the order of escaping "~" to "~0" and "/" to "~1" is important. */ -inline std::string escape(std::string s) +template +inline StringType escape(StringType s) { - replace_substring(s, "~", "~0"); - replace_substring(s, "/", "~1"); + replace_substring(s, StringType{"~"}, StringType{"~0"}); + replace_substring(s, StringType{"/"}, StringType{"~1"}); return s; } @@ -2754,24 +3002,36 @@ inline std::string escape(std::string s) * * Note the order of escaping "~1" to "/" and "~0" to "~" is important. */ -static void unescape(std::string& s) +template +static void unescape(StringType& s) { - replace_substring(s, "~1", "/"); - replace_substring(s, "~0", "~"); + replace_substring(s, StringType{"~1"}, StringType{"/"}); + replace_substring(s, StringType{"~0"}, StringType{"~"}); } -} // namespace detail -} // namespace nlohmann +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #include // size_t -namespace nlohmann -{ +// #include + + +NLOHMANN_JSON_NAMESPACE_BEGIN namespace detail { + /// struct to capture the start position of the current token struct position_t { @@ -2789,270 +3049,51 @@ struct position_t } }; -} // namespace detail -} // namespace nlohmann +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END // #include +// #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-FileCopyrightText: 2018 The Abseil Authors +// SPDX-License-Identifier: MIT -namespace nlohmann -{ + + +#include // array +#include // size_t +#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type +#include // index_sequence, make_index_sequence, index_sequence_for + +// #include + + +NLOHMANN_JSON_NAMESPACE_BEGIN namespace detail { -//////////////// -// exceptions // -//////////////// -/// @brief general exception of the @ref basic_json class -/// @sa https://json.nlohmann.me/api/basic_json/exception/ -class exception : public std::exception -{ - public: - /// returns the explanatory string - const char* what() const noexcept override - { - return m.what(); - } +template +using uncvref_t = typename std::remove_cv::type>::type; - /// the id of the exception - const int id; // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes) +#ifdef JSON_HAS_CPP_14 - protected: - JSON_HEDLEY_NON_NULL(3) - exception(int id_, const char* what_arg) : id(id_), m(what_arg) {} // NOLINT(bugprone-throw-keyword-missing) +// the following utilities are natively available in C++14 +using std::enable_if_t; +using std::index_sequence; +using std::make_index_sequence; +using std::index_sequence_for; - static std::string name(const std::string& ename, int id_) - { - return "[json.exception." + ename + "." + std::to_string(id_) + "] "; - } +#else - template - static std::string diagnostics(const BasicJsonType& leaf_element) - { -#if JSON_DIAGNOSTICS - std::vector tokens; - for (const auto* current = &leaf_element; current->m_parent != nullptr; current = current->m_parent) - { - switch (current->m_parent->type()) - { - case value_t::array: - { - for (std::size_t i = 0; i < current->m_parent->m_value.array->size(); ++i) - { - if (¤t->m_parent->m_value.array->operator[](i) == current) - { - tokens.emplace_back(std::to_string(i)); - break; - } - } - break; - } - - case value_t::object: - { - for (const auto& element : *current->m_parent->m_value.object) - { - if (&element.second == current) - { - tokens.emplace_back(element.first.c_str()); - break; - } - } - break; - } - - case value_t::null: // LCOV_EXCL_LINE - case value_t::string: // LCOV_EXCL_LINE - case value_t::boolean: // LCOV_EXCL_LINE - case value_t::number_integer: // LCOV_EXCL_LINE - case value_t::number_unsigned: // LCOV_EXCL_LINE - case value_t::number_float: // LCOV_EXCL_LINE - case value_t::binary: // LCOV_EXCL_LINE - case value_t::discarded: // LCOV_EXCL_LINE - default: // LCOV_EXCL_LINE - break; // LCOV_EXCL_LINE - } - } - - if (tokens.empty()) - { - return ""; - } - - return "(" + std::accumulate(tokens.rbegin(), tokens.rend(), std::string{}, - [](const std::string & a, const std::string & b) - { - return a + "/" + detail::escape(b); - }) + ") "; -#else - static_cast(leaf_element); - return ""; -#endif - } - - private: - /// an exception object as storage for error messages - std::runtime_error m; -}; - -/// @brief exception indicating a parse error -/// @sa https://json.nlohmann.me/api/basic_json/parse_error/ -class parse_error : public exception -{ - public: - /*! - @brief create a parse error exception - @param[in] id_ the id of the exception - @param[in] pos the position where the error occurred (or with - chars_read_total=0 if the position cannot be - determined) - @param[in] what_arg the explanatory string - @return parse_error object - */ - template - static parse_error create(int id_, const position_t& pos, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("parse_error", id_) + "parse error" + - position_string(pos) + ": " + exception::diagnostics(context) + what_arg; - return {id_, pos.chars_read_total, w.c_str()}; - } - - template - static parse_error create(int id_, std::size_t byte_, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("parse_error", id_) + "parse error" + - (byte_ != 0 ? (" at byte " + std::to_string(byte_)) : "") + - ": " + exception::diagnostics(context) + what_arg; - return {id_, byte_, w.c_str()}; - } - - /*! - @brief byte index of the parse error - - The byte index of the last read character in the input file. - - @note For an input with n bytes, 1 is the index of the first character and - n+1 is the index of the terminating null byte or the end of file. - This also holds true when reading a byte vector (CBOR or MessagePack). - */ - const std::size_t byte; - - private: - parse_error(int id_, std::size_t byte_, const char* what_arg) - : exception(id_, what_arg), byte(byte_) {} - - static std::string position_string(const position_t& pos) - { - return " at line " + std::to_string(pos.lines_read + 1) + - ", column " + std::to_string(pos.chars_read_current_line); - } -}; - -/// @brief exception indicating errors with iterators -/// @sa https://json.nlohmann.me/api/basic_json/invalid_iterator/ -class invalid_iterator : public exception -{ - public: - template - static invalid_iterator create(int id_, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("invalid_iterator", id_) + exception::diagnostics(context) + what_arg; - return {id_, w.c_str()}; - } - - private: - JSON_HEDLEY_NON_NULL(3) - invalid_iterator(int id_, const char* what_arg) - : exception(id_, what_arg) {} -}; - -/// @brief exception indicating executing a member function with a wrong type -/// @sa https://json.nlohmann.me/api/basic_json/type_error/ -class type_error : public exception -{ - public: - template - static type_error create(int id_, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("type_error", id_) + exception::diagnostics(context) + what_arg; - return {id_, w.c_str()}; - } - - private: - JSON_HEDLEY_NON_NULL(3) - type_error(int id_, const char* what_arg) : exception(id_, what_arg) {} -}; - -/// @brief exception indicating access out of the defined range -/// @sa https://json.nlohmann.me/api/basic_json/out_of_range/ -class out_of_range : public exception -{ - public: - template - static out_of_range create(int id_, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("out_of_range", id_) + exception::diagnostics(context) + what_arg; - return {id_, w.c_str()}; - } - - private: - JSON_HEDLEY_NON_NULL(3) - out_of_range(int id_, const char* what_arg) : exception(id_, what_arg) {} -}; - -/// @brief exception indicating other library errors -/// @sa https://json.nlohmann.me/api/basic_json/other_error/ -class other_error : public exception -{ - public: - template - static other_error create(int id_, const std::string& what_arg, const BasicJsonType& context) - { - std::string w = exception::name("other_error", id_) + exception::diagnostics(context) + what_arg; - return {id_, w.c_str()}; - } - - private: - JSON_HEDLEY_NON_NULL(3) - other_error(int id_, const char* what_arg) : exception(id_, what_arg) {} -}; - -} // namespace detail -} // namespace nlohmann - -// #include - -// #include - - -#include // size_t -#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type -#include // index_sequence, make_index_sequence, index_sequence_for - -// #include - - -namespace nlohmann -{ -namespace detail -{ - -template -using uncvref_t = typename std::remove_cv::type>::type; - -#ifdef JSON_HAS_CPP_14 - -// the following utilities are natively available in C++14 -using std::enable_if_t; -using std::index_sequence; -using std::make_index_sequence; -using std::index_sequence_for; - -#else - -// alias templates to reduce boilerplate -template -using enable_if_t = typename std::enable_if::type; +// alias templates to reduce boilerplate +template +using enable_if_t = typename std::enable_if::type; // The following code is taken from https://github.com/abseil/abseil-cpp/blob/10cb35e459f5ecca5b2ff107635da0bfa41011b4/absl/utility/utility.h // which is part of Google Abseil (https://github.com/abseil/abseil-cpp), licensed under the Apache License 2.0. @@ -3170,52 +3211,64 @@ template<> struct priority_tag<0> {}; template struct static_const { - static constexpr T value{}; + static JSON_INLINE_VARIABLE constexpr T value{}; }; -template -constexpr T static_const::value; // NOLINT(readability-redundant-declaration) - -} // namespace detail -} // namespace nlohmann - -// #include - +#ifndef JSON_HAS_CPP_17 + template + constexpr T static_const::value; +#endif -namespace nlohmann -{ -namespace detail +template +inline constexpr std::array make_array(Args&& ... args) { -// dispatching helper struct -template struct identity_tag {}; + return std::array {{static_cast(std::forward(args))...}}; +} + } // namespace detail -} // namespace nlohmann +NLOHMANN_JSON_NAMESPACE_END // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #include // numeric_limits #include // false_type, is_constructible, is_integral, is_same, true_type #include // declval #include // tuple - -// #include - +#include // char_traits // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #include // random_access_iterator_tag +// #include + // #include // #include -namespace nlohmann -{ +NLOHMANN_JSON_NAMESPACE_BEGIN namespace detail { + template struct iterator_types {}; @@ -3254,104 +3307,136 @@ struct iterator_traits::value>> using pointer = T*; using reference = T&; }; -} // namespace detail -} // namespace nlohmann + +} // namespace detail +NLOHMANN_JSON_NAMESPACE_END + +// #include // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + // #include -namespace nlohmann -{ +NLOHMANN_JSON_NAMESPACE_BEGIN + NLOHMANN_CAN_CALL_STD_FUNC_IMPL(begin); -} // namespace nlohmann + +NLOHMANN_JSON_NAMESPACE_END // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + // #include -namespace nlohmann -{ +NLOHMANN_JSON_NAMESPACE_BEGIN + NLOHMANN_CAN_CALL_STD_FUNC_IMPL(end); -} // namespace nlohmann + +NLOHMANN_JSON_NAMESPACE_END // #include // #include // #include +// __ _____ _____ _____ +// __| | __| | | | JSON for Modern C++ +// | | |__ | | | | | | version 3.11.3 +// |_____|_____|_____|_|___| https://github.com/nlohmann/json +// +// SPDX-FileCopyrightText: 2013-2023 Niels Lohmann +// SPDX-License-Identifier: MIT + #ifndef INCLUDE_NLOHMANN_JSON_FWD_HPP_ -#define INCLUDE_NLOHMANN_JSON_FWD_HPP_ + #define INCLUDE_NLOHMANN_JSON_FWD_HPP_ -#include // int64_t, uint64_t -#include // map -#include // allocator -#include // string -#include // vector + #include // int64_t, uint64_t + #include // map + #include // allocator + #include // string + #include // vector -/*! -@brief namespace for Niels Lohmann -@see https://github.com/nlohmann -@since version 1.0.0 -*/ -namespace nlohmann -{ -/*! -@brief default JSONSerializer template argument + // #include -This serializer ignores the template arguments and uses ADL -([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) -for serialization. -*/ -template -struct adl_serializer; - -/// a class to store JSON values -/// @sa https://json.nlohmann.me/api/basic_json/ -template class ObjectType = - std::map, - template class ArrayType = std::vector, - class StringType = std::string, class BooleanType = bool, - class NumberIntegerType = std::int64_t, - class NumberUnsignedType = std::uint64_t, - class NumberFloatType = double, - template class AllocatorType = std::allocator, - template class JSONSerializer = - adl_serializer, - class BinaryType = std::vector> -class basic_json; -/// @brief JSON Pointer defines a string syntax for identifying a specific value within a JSON document -/// @sa https://json.nlohmann.me/api/json_pointer/ -template -class json_pointer; + /*! + @brief namespace for Niels Lohmann + @see https://github.com/nlohmann + @since version 1.0.0 + */ + NLOHMANN_JSON_NAMESPACE_BEGIN -/*! -@brief default specialization -@sa https://json.nlohmann.me/api/json/ -*/ -using json = basic_json<>; + /*! + @brief default JSONSerializer template argument -/// @brief a minimal map-like container that preserves insertion order -/// @sa https://json.nlohmann.me/api/ordered_map/ -template -struct ordered_map; + This serializer ignores the template arguments and uses ADL + ([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) + for serialization. + */ + template + struct adl_serializer; + + /// a class to store JSON values + /// @sa https://json.nlohmann.me/api/basic_json/ + template class ObjectType = + std::map, + template class ArrayType = std::vector, + class StringType = std::string, class BooleanType = bool, + class NumberIntegerType = std::int64_t, + class NumberUnsignedType = std::uint64_t, + class NumberFloatType = double, + template class AllocatorType = std::allocator, + template class JSONSerializer = + adl_serializer, + class BinaryType = std::vector, // cppcheck-suppress syntaxError + class CustomBaseClass = void> + class basic_json; + + /// @brief JSON Pointer defines a string syntax for identifying a specific value within a JSON document + /// @sa https://json.nlohmann.me/api/json_pointer/ + template + class json_pointer; + + /*! + @brief default specialization + @sa https://json.nlohmann.me/api/json/ + */ + using json = basic_json<>; -/// @brief specialization that maintains the insertion order of object keys -/// @sa https://json.nlohmann.me/api/ordered_json/ -using ordered_json = basic_json; + /// @brief a minimal map-like container that preserves insertion order + /// @sa https://json.nlohmann.me/api/ordered_map/ + template + struct ordered_map; -} // namespace nlohmann + /// @brief specialization that maintains the insertion order of object keys + /// @sa https://json.nlohmann.me/api/ordered_json/ + using ordered_json = basic_json; + + NLOHMANN_JSON_NAMESPACE_END #endif // INCLUDE_NLOHMANN_JSON_FWD_HPP_ -namespace nlohmann -{ +NLOHMANN_JSON_NAMESPACE_BEGIN /*! @brief detail namespace with internal helper functions @@ -3362,6 +3447,7 @@ implementations of some @ref basic_json methods, and meta-programming helpers. */ namespace detail { + ///////////// // helpers // ///////////// @@ -3380,6 +3466,16 @@ template struct is_basic_json : std::false_type {}; NLOHMANN_BASIC_JSON_TPL_DECLARATION struct is_basic_json : std::true_type {}; +// used by exceptions create() member functions +// true_type for pointer to possibly cv-qualified basic_json or std::nullptr_t +// false_type otherwise +template +struct is_basic_json_context : + std::integral_constant < bool, + is_basic_json::type>::type>::value + || std::is_same::value > +{}; + ////////////////////// // json_ref helpers // ////////////////////// @@ -3481,17 +3577,92 @@ struct has_to_json < BasicJsonType, T, enable_if_t < !is_basic_json::value >> T>::value; }; +template +using detect_key_compare = typename T::key_compare; -/////////////////// -// is_ functions // -/////////////////// - -// https://en.cppreference.com/w/cpp/types/conjunction -template struct conjunction : std::true_type { }; -template struct conjunction : B1 { }; -template -struct conjunction -: std::conditional, B1>::type {}; +template +struct has_key_compare : std::integral_constant::value> {}; + +// obtains the actual object key comparator +template +struct actual_object_comparator +{ + using object_t = typename BasicJsonType::object_t; + using object_comparator_t = typename BasicJsonType::default_object_comparator_t; + using type = typename std::conditional < has_key_compare::value, + typename object_t::key_compare, object_comparator_t>::type; +}; + +template +using actual_object_comparator_t = typename actual_object_comparator::type; + +///////////////// +// char_traits // +///////////////// + +// Primary template of char_traits calls std char_traits +template +struct char_traits : std::char_traits +{}; + +// Explicitly define char traits for unsigned char since it is not standard +template<> +struct char_traits : std::char_traits +{ + using char_type = unsigned char; + using int_type = uint64_t; + + // Redefine to_int_type function + static int_type to_int_type(char_type c) noexcept + { + return static_cast(c); + } + + static char_type to_char_type(int_type i) noexcept + { + return static_cast(i); + } + + static constexpr int_type eof() noexcept + { + return static_cast(EOF); + } +}; + +// Explicitly define char traits for signed char since it is not standard +template<> +struct char_traits : std::char_traits +{ + using char_type = signed char; + using int_type = uint64_t; + + // Redefine to_int_type function + static int_type to_int_type(char_type c) noexcept + { + return static_cast(c); + } + + static char_type to_char_type(int_type i) noexcept + { + return static_cast(i); + } + + static constexpr int_type eof() noexcept + { + return static_cast(EOF); + } +}; + +/////////////////// +// is_ functions // +/////////////////// + +// https://en.cppreference.com/w/cpp/types/conjunction +template struct conjunction : std::true_type { }; +template struct conjunction : B { }; +template +struct conjunction +: std::conditional(B::value), conjunction, B>::type {}; // https://en.cppreference.com/w/cpp/types/negation template struct negation : std::integral_constant < bool, !B::value > { }; @@ -3518,7 +3689,6 @@ template struct is_default_constructible> : conjunction...> {}; - template struct is_constructible : std::is_constructible {}; @@ -3534,7 +3704,6 @@ struct is_constructible> : is_default_constructible struct is_constructible> : is_default_constructible> {}; - template struct is_iterator_traits : std::false_type {}; @@ -3655,9 +3824,18 @@ struct is_compatible_string_type template struct is_constructible_string_type { + // launder type through decltype() to fix compilation failure on ICPC +#ifdef __INTEL_COMPILER + using laundered_type = decltype(std::declval()); +#else + using laundered_type = ConstructibleStringType; +#endif + static constexpr auto value = - is_constructible::value; + conjunction < + is_constructible, + is_detected_exact>::value; }; template @@ -3678,164 +3856,817 @@ struct is_compatible_array_type_impl < range_value_t>::value; }; -template -struct is_compatible_array_type - : is_compatible_array_type_impl {}; +template +struct is_compatible_array_type + : is_compatible_array_type_impl {}; + +template +struct is_constructible_array_type_impl : std::false_type {}; + +template +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t::value >> + : std::true_type {}; + +template +struct is_constructible_array_type_impl < + BasicJsonType, ConstructibleArrayType, + enable_if_t < !std::is_same::value&& + !is_compatible_string_type::value&& + is_default_constructible::value&& +(std::is_move_assignable::value || + std::is_copy_assignable::value)&& +is_detected::value&& +is_iterator_traits>>::value&& +is_detected::value&& +// special case for types like std::filesystem::path whose iterator's value_type are themselves +// c.f. https://github.com/nlohmann/json/pull/3073 +!std::is_same>::value&& + is_complete_type < + detected_t>::value >> +{ + using value_type = range_value_t; + + static constexpr bool value = + std::is_same::value || + has_from_json::value || + has_non_default_from_json < + BasicJsonType, + value_type >::value; +}; + +template +struct is_constructible_array_type + : is_constructible_array_type_impl {}; + +template +struct is_compatible_integer_type_impl : std::false_type {}; + +template +struct is_compatible_integer_type_impl < + RealIntegerType, CompatibleNumberIntegerType, + enable_if_t < std::is_integral::value&& + std::is_integral::value&& + !std::is_same::value >> +{ + // is there an assert somewhere on overflows? + using RealLimits = std::numeric_limits; + using CompatibleLimits = std::numeric_limits; + + static constexpr auto value = + is_constructible::value && + CompatibleLimits::is_integer && + RealLimits::is_signed == CompatibleLimits::is_signed; +}; + +template +struct is_compatible_integer_type + : is_compatible_integer_type_impl {}; + +template +struct is_compatible_type_impl: std::false_type {}; + +template +struct is_compatible_type_impl < + BasicJsonType, CompatibleType, + enable_if_t::value >> +{ + static constexpr bool value = + has_to_json::value; +}; + +template +struct is_compatible_type + : is_compatible_type_impl {}; + +template +struct is_constructible_tuple : std::false_type {}; + +template +struct is_constructible_tuple> : conjunction...> {}; + +template +struct is_json_iterator_of : std::false_type {}; + +template +struct is_json_iterator_of : std::true_type {}; + +template +struct is_json_iterator_of : std::true_type +{}; + +// checks if a given type T is a template specialization of Primary +template