From c216de3bee98f008d3a6c1d6caf369128be0c619 Mon Sep 17 00:00:00 2001 From: admercs Date: Thu, 25 Apr 2024 16:03:14 -0700 Subject: [PATCH] taking out the trash --- .../include/common/AutonomySimSettings.hpp | 2168 ++++++++--------- 1 file changed, 1084 insertions(+), 1084 deletions(-) diff --git a/AutonomyLib/include/common/AutonomySimSettings.hpp b/AutonomyLib/include/common/AutonomySimSettings.hpp index e3b661b8..f10b4c9f 100644 --- a/AutonomyLib/include/common/AutonomySimSettings.hpp +++ b/AutonomyLib/include/common/AutonomySimSettings.hpp @@ -21,1315 +21,1315 @@ 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"; - // fields - float settings_version_actual; - float settings_version_minimum = 1.2f; + static constexpr char const *kVehicleInertialFrame = "VehicleInertialFrame"; + static constexpr char const *kSensorLocalFrame = "SensorLocalFrame"; - 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 constexpr char const *kSimModeTypeMultirotor = "Multirotor"; + static constexpr char const *kSimModeTypeCar = "Car"; + static constexpr char const *kSimModeTypeComputerVision = "ComputerVision"; - // 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: "; + struct SubwindowSetting { + int window_index; + ImageType image_type; + bool visible; + std::string camera_name; + std::string vehicle_name; + bool external; - error_messages.push_back(error_message + detected_keys); - } else - auto_upgrade = true; - } else - auto_upgrade = true; - } + 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 (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 - } + struct RecordingSetting { + bool record_on_move = false; + float record_interval = 0.05f; + std::string folder = ""; + bool enabled = false; - bool hasDefaultSettings(const Settings &settings_json, float &version) { - // if empty settings file - bool has_default = settings_json.size() == 0; + std::map> requests; - 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() {} - // If we have pre-V1 settings and only element is docs link - has_default |= settings_json.size() == 1 && has_docs; + 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 V1 settings and only elements are docs link and version - has_default |= settings_json.size() == 2 && has_docs && version > 0; + struct PawnPath { + std::string pawn_bp; + std::string slippery_mat; + std::string non_slippery_mat; - return has_default; - } + 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) {} + }; - 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 RCSettings { + int remote_control_id = -1; + bool allow_api_when_disconnected = false; + }; - 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 - } - } + struct Rotation { + float yaw = 0; + float pitch = 0; + float roll = 0; - void loadLevelSettings(const Settings &settings_json) { - level_name = settings_json.getString("Default Environment", ""); - } + Rotation() {} - void loadViewModeSettings(const Settings &settings_json) { - std::string view_mode_string = settings_json.getString("ViewMode", ""); + Rotation(float yaw_val, float pitch_val, float roll_val) : yaw(yaw_val), pitch(pitch_val), roll(roll_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"; + static Rotation nanRotation() noexcept { + static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); + return val; } + }; - 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); - } + struct GimbalSetting { + float stabilization = 0; + // bool is_world_frame = false; + Rotation rotation = Rotation::nanRotation(); + }; - 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); - } - } + 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; - 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))); - } + int image_type = 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)); - } - } + 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(); + }; - void loadRecordingSetting(const Settings &settings_json) { - loadDefaultRecordingSettings(); + struct NoiseSetting { + int ImageType = 0; - 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); + bool Enabled = false; - 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 RandContrib = 0.2f; + float RandSpeed = 100000.0f; + float RandSize = 500.0f; + float RandDensity = 2.0f; - for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { - Settings req_camera_settings; + float HorzWaveContrib = 0.03f; + float HorzWaveStrength = 0.08f; + float HorzWaveVertSize = 1.0f; + float HorzWaveScreenSize = 1.0f; - 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); - - recording_setting.requests[vehicle_name].push_back( - ImageCaptureBase::ImageRequest(camera_name, image_type, pixels_as_float, compress)); - } - } - } - } - } + float HorzNoiseLinesContrib = 1.0f; + float HorzNoiseLinesDensityY = 0.01f; + float HorzNoiseLinesDensityXY = 0.5f; - 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; - } + float HorzDistortionContrib = 1.0f; + float HorzDistortionStrength = 0.002f; + }; - 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 PixelFormatOverrideSetting { + int pixel_format = 0; + }; - 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 UnrealEngineSetting { + std::map pixel_format_override_settings; + }; - 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()); + 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(); - // 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; + GimbalSetting gimbal; + CaptureSettingsMap capture_settings; + NoiseSettingsMap noise_settings; - 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); + UnrealEngineSetting ue_setting; - 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); + CameraSetting() { + initializeCaptureSettings(capture_settings); + initializeNoiseSettings(noise_settings); + } + }; + using CameraSettingMap = std::map; - 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); + struct CameraDirectorSetting { + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + float follow_distance = Utils::nan(); + }; - 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); + 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. + }; - 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); + struct BarometerSetting : SensorSetting {}; - 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); + struct ImuSetting : SensorSetting {}; - 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 GpsSetting : SensorSetting {}; - connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); + struct MagnetometerSetting : SensorSetting {}; - 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); + struct DistanceSetting : SensorSetting {}; - 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); - } - } + struct LidarSetting : SensorSetting { + enum class DataFrame { VehicleInertialFrame, SensorLocalFrame }; + }; - return vehicle_setting_p; - } + struct VehicleSetting { + // required + std::string vehicle_name; + std::string vehicle_type; - 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", "")); + // 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; - 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; + // nan means use player start + Vector3r position = VectorMath::nanVector(); // in global NED + Rotation rotation = Rotation::nanRotation(); - // required settings_json - vehicle_setting->vehicle_type = vehicle_type; + CameraSettingMap cameras; + std::map> sensors; - // 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); + RCSettings rc; - loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); + VehicleSetting() {} - vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); - vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); + VehicleSetting(const std::string &vehicle_name_val, const std::string &vehicle_type_val) + : vehicle_name(vehicle_name_val), vehicle_type(vehicle_type_val) {} + }; - loadCameraSettings(settings_json, vehicle_setting->cameras, camera_defaults); - loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); + struct MavLinkConnectionInfo { + /* Default values are requires so uninitialized instance doesn't have random values */ - return vehicle_setting; - } + bool use_serial = true; // false means use UDP or TCP instead - static void createDefaultVehicle(const std::string &simmode_name, - std::map> &vehicles, - const std::map> &sensor_defaults) { - vehicles.clear(); + // Used to connect via HITL: needed only if use_serial = true + std::string serial_port = "*"; + int baud_rate = 115200; - // 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()); - } - } + // 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; - 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); + // 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; - nervosys::autonomylib::Settings vehicles_child; - if (settings_json.getChild("Vehicles", vehicles_child)) { - std::vector keys; - vehicles_child.getChildNames(keys); + // 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; - // remove default vehicles, if values are specified in settings - if (keys.size()) - vehicles.clear(); + // 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 = ""; - 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); - } - } - } + // 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 = ""; - 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'")); - } + // 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; - static void loadPawnPaths(const Settings &settings_json, std::map &pawn_paths) { - initializePawnPaths(pawn_paths); + // 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"; - nervosys::autonomylib::Settings pawn_paths_child; - if (settings_json.getChild("PawnPaths", pawn_paths_child)) { - std::vector keys; - pawn_paths_child.getChildNames(keys); + std::string model = "Generic"; - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - pawn_paths_child.getChild(key, child); - pawn_paths[key] = createPathPawn(child); - } - } - } + std::map params; + std::string logs; + }; - 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", ""); + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; - if (slippery_mat != "") - paths.slippery_mat = slippery_mat; - if (non_slippery_mat != "") - paths.non_slippery_mat = non_slippery_mat; + struct SegmentationSetting { + enum class InitMethodType { None, CommonObjectsRandomIDs }; - return paths; - } + enum class MeshNamingMethodType { OwnerName, StaticMeshName }; - 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); + InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; + bool override_existing = false; + MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; + }; - segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", true); + 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; + }; - 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); - } - } + // fields + std::string simmode_name = ""; + std::string level_name = ""; - 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(); - } + std::vector subwindow_settings; + RecordingSetting recording_setting; + SegmentationSetting segmentation_setting; + TimeOfDaySetting tod_setting; - 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 + std::vector warning_messages; + std::vector error_messages; - 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; - } - } - } - } + 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 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); + 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; - 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); + std::string settings_text_ = ""; + + // methods + static AutonomySimSettings &singleton() { + static AutonomySimSettings instance; + return instance; } - 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; + AutonomySimSettings() { + initializeSubwindowSettings(subwindow_settings); + initializePawnPaths(pawn_paths); } - 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(); + // 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); - 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; - } + 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 (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 - } - } - } - } + // this should be done last because it depends on vehicles (and/or their type) we have + loadRecordingSetting(settings_json); + loadClockSettings(settings_json); } - static CameraSetting createCameraSetting(const Settings &settings_json, const CameraSetting &camera_defaults) { - CameraSetting setting = camera_defaults; - - setting.position = createVectorSetting(settings_json, setting.position); - setting.rotation = createRotationSetting(settings_json, setting.rotation); - - 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); - - loadUnrealEngineSetting(settings_json, setting.ue_setting); - - return setting; + 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."); } - static void loadCameraSettings(const Settings &settings_json, CameraSettingMap &cameras, - const CameraSetting &camera_defaults) { - cameras.clear(); + static void createDefaultSettingsFile() { + initializeSettings("{}"); - Settings json_parent; - if (settings_json.getChild("Cameras", json_parent)) { - std::vector keys; - json_parent.getChildNames(keys); + 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); - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - json_parent.getChild(key, child); - cameras[key] = createCameraSetting(child, camera_defaults); - } - } + 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 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()); + // 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; - 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); + vehicle_setting->sensors = sensor_defaults; - capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); - } + VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, + vehicle_setting->rotation.yaw); - static void loadSubWindowsSettings(const Settings &settings_json, - std::vector &subwindow_settings) { - // load default subwindows - initializeSubwindowSettings(subwindow_settings); + vehicles[vehicle_name] = std::move(vehicle_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); - } - } - } + 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 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 + 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)); } - 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); + private: + typedef common_utils::Utils Utils; + typedef ImageCaptureBase::ImageType ImageType; - { // 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); - } - } + // fields + float settings_version_actual; + float settings_version_minimum = 1.2f; - { // 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); - } - } + 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; - { - // Wind Settings - Settings child_json; - if (settings_json.getChild("Wind", child_json)) { - wind = createVectorSetting(child_json, wind); + // 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: "; + + error_messages.push_back(error_message + detected_keys); + } else + auto_upgrade = true; + } else + auto_upgrade = true; } - } - { - // External Force Settings - Settings child_json; - if (settings_json.getChild("ExternalForce", child_json)) { - ext_force = createVectorSetting(child_json, ext_force); + + 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 } - 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); - } + bool hasDefaultSettings(const Settings &settings_json, float &version) { + // if empty settings file + bool has_default = settings_json.size() == 0; + + 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)); + + // If we have pre-V1 settings and only element is docs link + has_default |= settings_json.size() == 1 && has_docs; + + // if we have V1 settings and only elements are docs link and version + has_default |= settings_json.size() == 2 && has_docs && version > 0; + + return has_default; } - static void loadCameraDirectorSetting(const Settings &settings_json, CameraDirectorSetting &camera_director, - const std::string &simmode_name) { - camera_director = CameraDirectorSetting(); - 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); + 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"); } - if (std::isnan(camera_director.follow_distance)) { - if (simmode_name == kSimModeTypeCar) - camera_director.follow_distance = -8; + physics_engine_name = settings_json.getString("PhysicsEngineName", ""); + if (physics_engine_name == "") { + if (simmode_name == kSimModeTypeMultirotor) + physics_engine_name = "FastPhysicsEngine"; 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; + physics_engine_name = "PhysX"; // this value is only informational for now } } - void loadClockSettings(const Settings &settings_json) { - clock_type = settings_json.getString("ClockType", ""); - - if (clock_type == "") { - // default value - clock_type = "ScalableClock"; + void loadLevelSettings(const Settings &settings_json) { + level_name = settings_json.getString("Default Environment", ""); + } - // override if multirotor simmode with simple_flight - if (simmode_name == kSimModeTypeMultirotor) { - // TODO: this won't work if simple_flight and PX4 is combined together! + void loadViewModeSettings(const Settings &settings_json) { + std::string view_mode_string = settings_json.getString("ViewMode", ""); - // 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; - } - } - } + 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"; } - clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); + 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); } - static std::shared_ptr createSensorSetting(SensorBase::SensorType sensor_type, - const std::string &sensor_name, bool enabled) { - std::shared_ptr sensor_setting; - - 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"); + 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); } - - sensor_setting->sensor_type = sensor_type; - sensor_setting->sensor_name = sensor_name; - sensor_setting->enabled = enabled; - - return sensor_setting; } - static void initializeSensorSetting(SensorSetting *sensor_setting, const Settings &settings_json) { - sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); - - // 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; + 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))); } - // 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) + 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)); + } + } - { - // NOTE: Increase type if number of sensors goes above 8 - uint8_t present_sensors_bitmask = 0; + void loadRecordingSetting(const Settings &settings_json) { + loadDefaultRecordingSettings(); - nervosys::autonomylib::Settings sensors_child; - if (settings_json.getChild(collectionName, sensors_child)) { - std::vector keys; - sensors_child.getChildNames(keys); + 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); - for (const auto &key : keys) { - nervosys::autonomylib::Settings child; - sensors_child.getChild(key, child); + 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; - auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); - auto enabled = child.getBool("Enabled", false); + for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) { + Settings req_camera_settings; - sensors[key] = createSensorSetting(sensor_type, key, enabled); - initializeSensorSetting(sensors[key].get(), child); + 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); - // Mark sensor types already added - present_sensors_bitmask |= 1U << Utils::toNumeric(sensor_type); + recording_setting.requests[vehicle_name].push_back( + ImageCaptureBase::ImageRequest(camera_name, image_type, pixels_as_float, compress)); + } + } } } - - // 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; - } } - // 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 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; } - // 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 loadExternalCameraSettings(const Settings &settings_json, CameraSettingMap &external_cameras, - const CameraSetting &camera_defaults) { - external_cameras.clear(); + 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 json_parent; - if (settings_json.getChild("ExternalCameras", json_parent)) { - std::vector keys; - json_parent.getChildNames(keys); - - for (const auto &key : keys) { - Settings child; - json_parent.getChild(key, child); - external_cameras[key] = createCameraSetting(child, camera_defaults); + 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; + } } } } - 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 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 constexpr char const *kVehicleInertialFrame = "VehicleInertialFrame"; - static constexpr char const *kSensorLocalFrame = "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; - static constexpr char const *kSimModeTypeMultirotor = "Multirotor"; - static constexpr char const *kSimModeTypeCar = "Car"; - static constexpr char const *kSimModeTypeComputerVision = "ComputerVision"; + 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); - struct SubwindowSetting { - int window_index; - ImageType image_type; - bool visible; - std::string camera_name; - std::string vehicle_name; - bool external; - - 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) {} - }; - - struct RecordingSetting { - bool record_on_move = false; - float record_interval = 0.05f; - std::string folder = ""; - bool enabled = false; - - std::map> requests; - - RecordingSetting() {} + 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); - 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) {} - }; + 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); - struct PawnPath { - std::string pawn_bp; - std::string slippery_mat; - std::string non_slippery_mat; + 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); - 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) {} - }; + 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); - struct RCSettings { - int remote_control_id = -1; - bool allow_api_when_disconnected = false; - }; + 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); - struct Rotation { - float yaw = 0; - float pitch = 0; - float roll = 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); + } - Rotation() {} + connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip); - Rotation(float yaw_val, float pitch_val, float roll_val) : yaw(yaw_val), pitch(pitch_val), roll(roll_val) {} + 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); - static Rotation nanRotation() noexcept { - static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); - return val; + 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); + } } - }; - - 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; + return vehicle_setting_p; + } - int image_type = 0; + 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", "")); - 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(); - }; + 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; - struct NoiseSetting { - int ImageType = 0; + // required settings_json + vehicle_setting->vehicle_type = vehicle_type; - bool Enabled = false; + // 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); - float RandContrib = 0.2f; - float RandSpeed = 100000.0f; - float RandSize = 500.0f; - float RandDensity = 2.0f; + loadRCSetting(simmode_name, settings_json, vehicle_setting->rc); - float HorzWaveContrib = 0.03f; - float HorzWaveStrength = 0.08f; - float HorzWaveVertSize = 1.0f; - float HorzWaveScreenSize = 1.0f; + vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position); + vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation); - float HorzNoiseLinesContrib = 1.0f; - float HorzNoiseLinesDensityY = 0.01f; - float HorzNoiseLinesDensityXY = 0.5f; + loadCameraSettings(settings_json, vehicle_setting->cameras, camera_defaults); + loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults); - float HorzDistortionContrib = 1.0f; - float HorzDistortionStrength = 0.002f; - }; + return vehicle_setting; + } - struct PixelFormatOverrideSetting { - int pixel_format = 0; - }; + static void createDefaultVehicle(const std::string &simmode_name, + std::map> &vehicles, + const std::map> &sensor_defaults) { + vehicles.clear(); - struct UnrealEngineSetting { - std::map pixel_format_override_settings; - }; + // 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()); + } + } - 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(); + 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); - GimbalSetting gimbal; - CaptureSettingsMap capture_settings; - NoiseSettingsMap noise_settings; + nervosys::autonomylib::Settings vehicles_child; + if (settings_json.getChild("Vehicles", vehicles_child)) { + std::vector keys; + vehicles_child.getChildNames(keys); - UnrealEngineSetting ue_setting; + // remove default vehicles, if values are specified in settings + if (keys.size()) + vehicles.clear(); - CameraSetting() { - initializeCaptureSettings(capture_settings); - initializeNoiseSettings(noise_settings); + 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); + } } - }; - using CameraSettingMap = std::map; + } - struct CameraDirectorSetting { - Vector3r position = VectorMath::nanVector(); - Rotation rotation = Rotation::nanRotation(); - float follow_distance = Utils::nan(); - }; + 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'")); + } - 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. - }; + static void loadPawnPaths(const Settings &settings_json, std::map &pawn_paths) { + initializePawnPaths(pawn_paths); - struct BarometerSetting : SensorSetting {}; + nervosys::autonomylib::Settings pawn_paths_child; + if (settings_json.getChild("PawnPaths", pawn_paths_child)) { + std::vector keys; + pawn_paths_child.getChildNames(keys); - struct ImuSetting : SensorSetting {}; + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + pawn_paths_child.getChild(key, child); + pawn_paths[key] = createPathPawn(child); + } + } + } - struct GpsSetting : SensorSetting {}; + 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", ""); - struct MagnetometerSetting : SensorSetting {}; + if (slippery_mat != "") + paths.slippery_mat = slippery_mat; + if (non_slippery_mat != "") + paths.non_slippery_mat = non_slippery_mat; - struct DistanceSetting : SensorSetting {}; + return paths; + } - struct LidarSetting : SensorSetting { - enum class DataFrame { VehicleInertialFrame, SensorLocalFrame }; - }; + 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); - struct VehicleSetting { - // required - std::string vehicle_name; - std::string vehicle_type; + segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", true); - // 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; + 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); + } + } - // nan means use player start - Vector3r position = VectorMath::nanVector(); // in global NED - Rotation rotation = Rotation::nanRotation(); + 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(); + } - CameraSettingMap cameras; - std::map> sensors; + 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 - RCSettings rc; + 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; + } + } + } + } - VehicleSetting() {} + 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); - VehicleSetting(const std::string &vehicle_name_val, const std::string &vehicle_type_val) - : vehicle_name(vehicle_name_val), vehicle_type(vehicle_type_val) {} - }; + 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); + } - struct MavLinkConnectionInfo { - /* Default values are requires so uninitialized instance doesn't have random values */ + 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; + } - bool use_serial = true; // false means use UDP or TCP instead + 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(); - // Used to connect via HITL: needed only if use_serial = true - std::string serial_port = "*"; - int baud_rate = 115200; + 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; + } - // 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; + 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 + } + } + } + } + } - // 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 CameraSetting createCameraSetting(const Settings &settings_json, const CameraSetting &camera_defaults) { + CameraSetting setting = camera_defaults; - // 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; + setting.position = createVectorSetting(settings_json, setting.position); + setting.rotation = createRotationSetting(settings_json, setting.rotation); - // 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 = ""; + 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); - // 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 = ""; + loadUnrealEngineSetting(settings_json, setting.ue_setting); - // 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; + return setting; + } - // 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"; + static void loadCameraSettings(const Settings &settings_json, CameraSettingMap &cameras, + const CameraSetting &camera_defaults) { + cameras.clear(); - std::string model = "Generic"; + Settings json_parent; + if (settings_json.getChild("Cameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); - std::map params; - std::string logs; - }; + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + json_parent.getChild(key, child); + cameras[key] = createCameraSetting(child, camera_defaults); + } + } + } - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + 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 SegmentationSetting { - enum class InitMethodType { None, CommonObjectsRandomIDs }; + 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); - enum class MeshNamingMethodType { OwnerName, StaticMeshName }; + capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width); + } - InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs; - bool override_existing = false; - MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName; - }; + static void loadSubWindowsSettings(const Settings &settings_json, + std::vector &subwindow_settings) { + // load default subwindows + initializeSubwindowSettings(subwindow_settings); - 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; - }; + 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); + } + } + } + } + + 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 + } + + 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); + + { // 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); + } + } + + { // 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 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(); - // fields - std::string simmode_name = ""; - std::string level_name = ""; + 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); + } - std::vector subwindow_settings; - RecordingSetting recording_setting; - SegmentationSetting segmentation_setting; - TimeOfDaySetting tod_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; + } + } - std::vector warning_messages; - std::vector error_messages; + void loadClockSettings(const Settings &settings_json) { + clock_type = settings_json.getString("ClockType", ""); - 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 = ""; + if (clock_type == "") { + // default value + clock_type = "ScalableClock"; - 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; + // 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 settings_text_ = ""; + // 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; + } + } + } + } - // methods - static AutonomySimSettings &singleton() { - static AutonomySimSettings instance; - return instance; + clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); } - AutonomySimSettings() { - initializeSubwindowSettings(subwindow_settings); - initializePawnPaths(pawn_paths); - } + static std::shared_ptr createSensorSetting(SensorBase::SensorType sensor_type, + const std::string &sensor_name, bool enabled) { + std::shared_ptr sensor_setting; - // 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); + 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"); + } - 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); + sensor_setting->sensor_type = sensor_type; + sensor_setting->sensor_name = sensor_name; + sensor_setting->enabled = enabled; - // this should be done last because it depends on vehicles (and/or their type) we have - loadRecordingSetting(settings_json); - loadClockSettings(settings_json); + return sensor_setting; } - 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."); + static void initializeSensorSetting(SensorSetting *sensor_setting, const Settings &settings_json) { + sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled); + + // 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; } - static void createDefaultSettingsFile() { - initializeSettings("{}"); + // 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) - 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); + { + // NOTE: Increase type if number of sensors goes above 8 + uint8_t present_sensors_bitmask = 0; - 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); - } + nervosys::autonomylib::Settings sensors_child; + if (settings_json.getChild(collectionName, sensors_child)) { + std::vector keys; + sensors_child.getChildNames(keys); - // 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; + for (const auto &key : keys) { + nervosys::autonomylib::Settings child; + sensors_child.getChild(key, child); - vehicle_setting->sensors = sensor_defaults; + auto sensor_type = Utils::toEnum(child.getInt("SensorType", 0)); + auto enabled = child.getBool("Enabled", false); - VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, - vehicle_setting->rotation.yaw); + sensors[key] = createSensorSetting(sensor_type, key, enabled); + initializeSensorSetting(sensors[key].get(), child); - vehicles[vehicle_name] = std::move(vehicle_setting); + // Mark sensor types already added + present_sensors_bitmask |= 1U << Utils::toNumeric(sensor_type); + } + } + + // 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; + } } - 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(); + // 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 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())); + // 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 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 loadExternalCameraSettings(const Settings &settings_json, CameraSettingMap &external_cameras, + const CameraSetting &camera_defaults) { + external_cameras.clear(); + + Settings json_parent; + if (settings_json.getChild("ExternalCameras", json_parent)) { + std::vector keys; + json_parent.getChildNames(keys); + + for (const auto &key : keys) { + Settings child; + json_parent.getChild(key, child); + external_cameras[key] = createCameraSetting(child, camera_defaults); + } + } } };