From 0fe6b486e1796cd22d8978b818bf458ade165ca5 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Thu, 22 Feb 2018 16:05:24 +0100 Subject: [PATCH 001/121] Add a setting to disable main viewport rendering This frees up cycles to render the subwindows or serve images through the API. --- AirLib/include/common/AirSimSettings.hpp | 5 ++++- Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 7 +++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index efe848c9a4..b66cf4a3ae 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -167,6 +167,7 @@ struct AirSimSettings { float clock_speed; bool engine_sound; bool log_messages_visible; + bool main_viewport_visible; HomeGeoPoint origin_geopoint; public: //methods @@ -226,6 +227,7 @@ struct AirSimSettings { clock_speed = 1.0f; engine_sound = true; log_messages_visible = true; + main_viewport_visible = true; //0,0,0 in Unreal is mapped to this GPS coordinates origin_geopoint = HomeGeoPoint(GeoPoint(47.641468, -122.140165, 122)); } @@ -552,6 +554,7 @@ struct AirSimSettings { enable_collision_passthrough = settings.getBool("EnableCollisionPassthrogh", false); log_messages_visible = settings.getBool("LogMessagesVisible", true); + main_viewport_visible = settings.getBool("MainViewportVisible", true); { //load origin geopoint Settings origin_geopoint_json; @@ -592,4 +595,4 @@ struct AirSimSettings { }; }} //namespace -#endif \ No newline at end of file +#endif diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 8e42596467..466365529e 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -205,6 +205,13 @@ void ASimHUD::setUnrealEngineSettings() static const auto render_timeout_var = IConsoleManager::Get().FindConsoleVariable(TEXT("g.TimeoutForBlockOnRenderFence")); render_timeout_var->Set(300000); + const bool main_viewport_visible = AirSimSettings::singleton().main_viewport_visible; + + if (!main_viewport_visible) { + // This disables rendering of the main viewport in the same way as the + // console command "show rendering" would do. + GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(false); + } } void ASimHUD::setupInputBindings() From d37669f64eba012f3742b592f0ba3e6f6edbb458 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Thu, 22 Feb 2018 16:06:23 +0100 Subject: [PATCH 002/121] Flush render queue after draw if main viewport is not visible This allows the API to capture every single frame. Otherwise, after an API image request is received, it would wait until after both the current frame and the subsequent frames are finished rendering, effectively only capturing every other frame when an API user captures pictures in a loop. When flushing the render queue after the viewport has been drawn, the render request is executed once the current frame is available. Only enable this behavior when main viewport drawing is disabled. This makes sure we don't adversely affect main viewport drawing performance by adding more otherwise unneeded synchronization. --- Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 466365529e..1fe876e4dc 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -4,6 +4,7 @@ #include "Car/SimModeCar.h" #include "common/AirSimSettings.hpp" #include "Kismet/KismetSystemLibrary.h" +#include "Runtime/Engine/Public/Slate/SceneViewport.h" #include @@ -211,6 +212,16 @@ void ASimHUD::setUnrealEngineSettings() // This disables rendering of the main viewport in the same way as the // console command "show rendering" would do. GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(false); + + // When getting an image through the API, the image is produced after the render + // thread has finished rendering the current and the subsequent frame. This means + // that the frame rate for obtaining images through the API is only half as high as + // it could be, since only every other image is actually captured. We work around + // this by telling the viewport to flush the rendering queue at the end of each + // drawn frame so that it executes our render request at that point already. + // Do this only if the main viewport is not being rendered anyway in case there are + // any adverse performance effects during main rendering. + GetWorld()->GetGameViewport()->GetGameViewport()->IncrementFlushOnDraw(); } } From e8936ee32f0cba400f677fbbce2e7f6fef1c87e4 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Thu, 22 Feb 2018 16:15:11 +0100 Subject: [PATCH 003/121] Optimize image copying loop after capturing an image This improves the runtime of this loop from about 10ms to less than 1ms on my computer. When repeatedly obtaining images through the API in a loop, this image copying does not seem to be in the critical path (probably because the GPU already starts rendering the next frame while the copy is going on), but it seems to be some low-hanging fruit nonetheless. --- Unreal/Plugins/AirSim/Source/RenderRequest.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp index 5066ac5f3f..27767b10f8 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp @@ -69,22 +69,25 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v for (unsigned int i = 0; i < req_size; ++i) { if (! params[i]->pixels_as_float) { if (results[i]->width != 0 && results[i]->height != 0) { + results[i]->image_data_uint8.SetNumUninitialized(results[i]->width * results[i]->height * 4, false); if (params[i]->compress) FImageUtils::CompressImageArray(results[i]->width, results[i]->height, results[i]->bmp, results[i]->image_data_uint8); else { + uint8* ptr = results[i]->image_data_uint8.GetData(); for (const auto& item : results[i]->bmp) { - results[i]->image_data_uint8.Add(item.R); - results[i]->image_data_uint8.Add(item.G); - results[i]->image_data_uint8.Add(item.B); - results[i]->image_data_uint8.Add(item.A); + *ptr++ = item.R; + *ptr++ = item.G; + *ptr++ = item.B; + *ptr++ = item.A; } } } } else { + results[i]->image_data_float.SetNumUninitialized(results[i]->width * results[i]->height); + float* ptr = results[i]->image_data_float.GetData(); for (const auto& item : results[i]->bmp_float) { - float fval = item.R.GetFloat(); - results[i]->image_data_float.Add(fval); + *ptr++ = item.R.GetFloat(); } } } From 85ac643bd1e89f7481e5e2b04408b06efca553ac Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 27 Feb 2018 14:28:31 +0000 Subject: [PATCH 004/121] fixed https://github.com/Microsoft/AirSim/issues/844 --- Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index a53e9572a7..1a12fecf45 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -16,6 +16,7 @@ //#include "Runtime/Foliage/Public/FoliageType.h" #include "Kismet/KismetStringLibrary.h" #include "MessageDialog.h" +#include "Engine/LocalPlayer.h" #include "Engine/Engine.h" /* From 20361036435065b3d29842a61d81a3d58459078e Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Tue, 27 Feb 2018 17:42:53 +0100 Subject: [PATCH 005/121] Move the setting to the CameraDirector's view_mode --- AirLib/include/common/AirSimSettings.hpp | 3 -- .../Plugins/AirSim/Source/CameraDirector.cpp | 39 ++++++++++++++++--- Unreal/Plugins/AirSim/Source/CameraDirector.h | 4 +- .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 19 --------- 4 files changed, 35 insertions(+), 30 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index b66cf4a3ae..b564888b4e 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -167,7 +167,6 @@ struct AirSimSettings { float clock_speed; bool engine_sound; bool log_messages_visible; - bool main_viewport_visible; HomeGeoPoint origin_geopoint; public: //methods @@ -227,7 +226,6 @@ struct AirSimSettings { clock_speed = 1.0f; engine_sound = true; log_messages_visible = true; - main_viewport_visible = true; //0,0,0 in Unreal is mapped to this GPS coordinates origin_geopoint = HomeGeoPoint(GeoPoint(47.641468, -122.140165, 122)); } @@ -554,7 +552,6 @@ struct AirSimSettings { enable_collision_passthrough = settings.getBool("EnableCollisionPassthrogh", false); log_messages_visible = settings.getBool("LogMessagesVisible", true); - main_viewport_visible = settings.getBool("MainViewportVisible", true); { //load origin geopoint Settings origin_geopoint_json; diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index 4e4fd878f2..a56d76881c 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -1,5 +1,6 @@ #include "CameraDirector.h" #include "GameFramework/PlayerController.h" +#include "Runtime/Engine/Public/Slate/SceneViewport.h" #include "AirBlueprintLib.h" ACameraDirector::ACameraDirector() @@ -34,7 +35,7 @@ void ACameraDirector::Tick(float DeltaTime) else if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE) { //do nothing, spring arm is pulling the camera with it } - else if (mode_ == ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY) { + else if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) { //do nothing, we have camera turned off } else { //make camera move in desired way @@ -92,8 +93,8 @@ void ACameraDirector::setCameras(APIPCamera* external_camera, VehiclePawnWrapper case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER: inputEventGroundView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL: inputEventManualView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE: inputEventSpringArmChaseView(); break; - case ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP: inputEventBackupView(); break; - case ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY: inputEventNoDisplayView(); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_BACKUP: inputEventBackupView(); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY: inputEventNoDisplayView(); break; default: throw std::out_of_range("Unsupported view mode specified in CameraDirector::initializeForBeginPlay"); } @@ -146,6 +147,32 @@ void ACameraDirector::setMode(ECameraDirectorMode mode) attachSpringArm(false); } + // Enable/disable primary viewport rendering flag + if (mode_ != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY && + mode == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) + { + // This disables rendering of the main viewport in the same way as the + // console command "show rendering" would do. + GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(false); + + // When getting an image through the API, the image is produced after the render + // thread has finished rendering the current and the subsequent frame. This means + // that the frame rate for obtaining images through the API is only half as high as + // it could be, since only every other image is actually captured. We work around + // this by telling the viewport to flush the rendering queue at the end of each + // drawn frame so that it executes our render request at that point already. + // Do this only if the main viewport is not being rendered anyway in case there are + // any adverse performance effects during main rendering. + GetWorld()->GetGameViewport()->GetGameViewport()->IncrementFlushOnDraw(); + } + else if(mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY && + mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) + { + // Re-enable rendering + GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(true); + GetWorld()->GetGameViewport()->GetGameViewport()->DecrementFlushOnDraw(); + } + mode_ = mode; //if new mode is manual mode then add key bindings @@ -171,7 +198,7 @@ void ACameraDirector::setupInputBindings() UAirBlueprintLib::BindActionToKey("inputEventManualView", EKeys::M, this, &ACameraDirector::inputEventManualView); UAirBlueprintLib::BindActionToKey("inputEventSpringArmChaseView", EKeys::Slash, this, &ACameraDirector::inputEventSpringArmChaseView); UAirBlueprintLib::BindActionToKey("inputEventBackupView", EKeys::K, this, &ACameraDirector::inputEventBackupView); - UAirBlueprintLib::BindActionToKey("inputEventNoDispalyView", EKeys::Hyphen, this, &ACameraDirector::inputEventNoDisplayView); + UAirBlueprintLib::BindActionToKey("inputEventNoDisplayView", EKeys::Hyphen, this, &ACameraDirector::inputEventNoDisplayView); } @@ -229,13 +256,13 @@ void ACameraDirector::inputEventManualView() void ACameraDirector::inputEventNoDisplayView() { - setMode(ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY); + setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY); disableCameras(true, true, true); } void ACameraDirector::inputEventBackupView() { - setMode(ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP); + setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_BACKUP); external_camera_->disableMain(); if (fpv_camera_) fpv_camera_->disableMain(); diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.h b/Unreal/Plugins/AirSim/Source/CameraDirector.h index a2c4cfa2d8..51f0857903 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.h +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.h @@ -18,8 +18,8 @@ enum class ECameraDirectorMode : uint8 CAMERA_DIRECTOR_MODE_FLY_WITH_ME = 3 UMETA(DisplayName = "FlyWithMe"), CAMERA_DIRECTOR_MODE_MANUAL = 4 UMETA(DisplayName = "Manual"), CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE = 5 UMETA(DisplayName = "SpringArmChase"), - CAMREA_DIRECTOR_MODE_BACKUP = 6 UMETA(DisplayName = "Backup"), - CAMREA_DIRECTOR_MODE_NODISPLAY = 7 UMETA(DisplayName = "No Display") + CAMERA_DIRECTOR_MODE_BACKUP = 6 UMETA(DisplayName = "Backup"), + CAMERA_DIRECTOR_MODE_NODISPLAY = 7 UMETA(DisplayName = "No Display") }; UCLASS() diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 1fe876e4dc..ccb89f3f91 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -4,7 +4,6 @@ #include "Car/SimModeCar.h" #include "common/AirSimSettings.hpp" #include "Kismet/KismetSystemLibrary.h" -#include "Runtime/Engine/Public/Slate/SceneViewport.h" #include @@ -205,24 +204,6 @@ void ASimHUD::setUnrealEngineSettings() //we get error that GameThread has timed out after 30 sec waiting on render thread static const auto render_timeout_var = IConsoleManager::Get().FindConsoleVariable(TEXT("g.TimeoutForBlockOnRenderFence")); render_timeout_var->Set(300000); - - const bool main_viewport_visible = AirSimSettings::singleton().main_viewport_visible; - - if (!main_viewport_visible) { - // This disables rendering of the main viewport in the same way as the - // console command "show rendering" would do. - GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(false); - - // When getting an image through the API, the image is produced after the render - // thread has finished rendering the current and the subsequent frame. This means - // that the frame rate for obtaining images through the API is only half as high as - // it could be, since only every other image is actually captured. We work around - // this by telling the viewport to flush the rendering queue at the end of each - // drawn frame so that it executes our render request at that point already. - // Do this only if the main viewport is not being rendered anyway in case there are - // any adverse performance effects during main rendering. - GetWorld()->GetGameViewport()->GetGameViewport()->IncrementFlushOnDraw(); - } } void ASimHUD::setupInputBindings() From 37aabb1e5800a267c13d91998cd9f6c15f9a563c Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 1 Mar 2018 12:34:32 +0530 Subject: [PATCH 006/121] fixed https://github.com/Microsoft/AirSim/issues/848 --- AirLib/include/sensors/distance/DistanceSimple.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 432cd7ab82..201db6dcb6 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -75,7 +75,8 @@ class DistanceSimple : public DistanceBase { Output output; const GroundTruth& ground_truth = getGroundTruth(); - auto distance = getRayLength(ground_truth.kinematics->pose + params_.relative_pose); + //order of Pose addition is important here because it also adds quaternions which is not commutative! + auto distance = getRayLength(params_.relative_pose + ground_truth.kinematics->pose); //add noise in distance (about 0.2m sigma) distance += uncorrelated_noise_.next(); From a24bfe2cbcf709293ce90e4fe2c07c3d8c5e1317 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 00:18:57 +0530 Subject: [PATCH 007/121] minor fixes for TimeOfDay --- AirLib/include/common/AirSimSettings.hpp | 2 +- AirLib/include/common/SteppableClock.hpp | 4 +- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 3 + .../AirSim/Source/SimMode/SimModeBase.cpp | 7 ++- docs/settings.md | 58 ++++++++++++------- 5 files changed, 48 insertions(+), 26 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index efe848c9a4..60c625e746 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -559,7 +559,7 @@ struct AirSimSettings { GeoPoint origin = origin_geopoint.home_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("Latitude", origin.altitude); + origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude); origin_geopoint.initialize(origin); } } diff --git a/AirLib/include/common/SteppableClock.hpp b/AirLib/include/common/SteppableClock.hpp index 85694be629..829829d502 100644 --- a/AirLib/include/common/SteppableClock.hpp +++ b/AirLib/include/common/SteppableClock.hpp @@ -16,10 +16,10 @@ class SteppableClock : public ClockBase { //TTimePoint is nano seconds passed since some fixed point in time //step is how much we would advance the clock by default when calling step() //by default we advance by 20ms - SteppableClock(TTimeDelta step = 20E-3f, TTimePoint start = 1000) + SteppableClock(TTimeDelta step = 20E-3f, TTimePoint start = 0) : current_(start), step_(step) { - current_ = start; + current_ = start ? start : Utils::getTimeSinceEpochNanos(); } TTimePoint stepBy(TTimeDelta amount) diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 65af82002c..d5bdb92566 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -51,6 +51,9 @@ void APIPCamera::BeginPlay() { Super::BeginPlay(); + //Computer transform using the actor that this camera is attached to + //if this is free floting camera like external observer camera then just use + //itself to select origin point auto* p = this->GetAttachParentActor(); ned_transform_.initialize(p ? p : this); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 94a01cb2f8..1ac5250da9 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -97,7 +97,7 @@ void ASimModeBase::setupTimeOfDay() //} tod_sim_clock_start_ = ClockFactory::get()->nowNanos(); - tod_last_update_ = tod_sim_clock_start_; + tod_last_update_ = 0; if (tod_settings.start_datetime != "") tod_start_time_ = Utils::to_time_t(tod_settings.start_datetime, tod_settings.is_start_datetime_dst); else @@ -161,9 +161,10 @@ void ASimModeBase::Tick(float DeltaSeconds) void ASimModeBase::advanceTimeOfDay() { - if (sky_sphere_ && sun_) { + const auto& settings = getSettings(); + + if (settings.tod_settings.enabled && sky_sphere_ && sun_) { auto secs = ClockFactory::get()->elapsedSince(tod_last_update_); - const auto& settings = getSettings(); if (secs > settings.tod_settings.update_interval_secs) { tod_last_update_ = ClockFactory::get()->nowNanos(); diff --git a/docs/settings.md b/docs/settings.md index 6a1998ddee..35ca0b85fc 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -61,6 +61,38 @@ Below are complete list of settings available along with their default values. I "OrthoWidth": 5.12 } ], + "OriginGeopoint": { + "Latitude": 47.641468, + "Latitude": -122.140165, + "Altitude": 122 + }, + "TimeOfDay": { + "Enabled": false, + "StartDateTime": "", + "CelestialClockSpeed": 1, + "StartDateTimeDst": false, + "UpdateIntervalSecs": 60 + }, + "SubWindows": [ + {"WindowID": 0, "CameraID": 0, "ImageType": 3, "Visible": false}, + {"WindowID": 1, "CameraID": 0, "ImageType": 5, "Visible": false}, + {"WindowID": 2, "CameraID": 0, "ImageType": 0, "Visible": false} + ], + "SimpleFlight": { + "FirmwareName": "SimpleFlight", + "DefaultVehicleState": "Armed", + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false, + "AllowAPIAlways": true + }, + "ApiServerPort": 41451 + }, + "SegmentationSettings": { + "InitMethod": "", + "MeshNamingMethod": "", + "OverrideExisting": false + }, "NoiseSettings": [ { "Enabled": false, @@ -84,26 +116,6 @@ Below are complete list of settings available along with their default values. I "HorzDistortionStrength": 0.002 } ], - "SubWindows": [ - {"WindowID": 0, "CameraID": 0, "ImageType": 3, "Visible": false}, - {"WindowID": 1, "CameraID": 0, "ImageType": 5, "Visible": false}, - {"WindowID": 2, "CameraID": 0, "ImageType": 0, "Visible": false} - ], - "SimpleFlight": { - "FirmwareName": "SimpleFlight", - "DefaultVehicleState": "Armed", - "RC": { - "RemoteControlID": 0, - "AllowAPIWhenDisconnected": false, - "AllowAPIAlways": true - }, - "ApiServerPort": 41451 - }, - "SegmentationSettings": { - "InitMethod": "", - "MeshNamingMethod": "", - "OverrideExisting": false - }, "PX4": { "FirmwareName": "PX4", "LogViewerHostIp": "127.0.0.1", @@ -167,6 +179,12 @@ The ViewMode determines how you will view the vehicle. For multirotors, the defa * Manual: Don't move camera automatically. Use arrow keys and ASWD keys for move camera manually. * SpringArmChase: Chase the vehicle with camera mounted on (invisible) arm that is attached to the vehicle via spring (so it has some latency in movement). +### TimeOfDay +This section of settings controls the position of Sun in the environment. By default `Enabled` is false which means Sun's position is left at whatever was the default in the environment and it doesn't change over the time. If `Enabled` is true then Sun position is computed using longitude, latitude and altitude specified in `OriginGeopoint` section for the date specified in `StartDateTime` in the string format as `%Y-%m-%d %H:%M:%S`, for example, `2018-02-12 15:20:00`. If this string is empty then current date and time is used. If `StartDateTimeDst` is true then we adjust for day light savings time. The Sun's position is then contibuously updated at the interval specified in `UpdateIntervalSecs`. In some cases, it might be desirable to have celestial clock run faster or slower than simulation clock. This can be specified using `CelestialClockSpeed`, for example, value 100 means for every 1 second of simulation clock, Sun's position is advanced by 100 seconds so Sun will move in sky much faster. + +### OriginGeopoint +This specifies the latitude, longitude and altitude of the coordinates (0, 0, 0) in Unreal Units in the Unreal Engine environment. The vehicle's home point is computed using this transformation. Note that all coordinates exposed via APIs are using NED system in SI units which means each vehicle starts at (0, 0, 0) in NED system. Time of Day settings are computed at (0, 0, 0) coordinates in Unreal Engine environment. This means that Sun position is computed for geographical coordinates specified in `OriginGeopoint` and time specified in `TimeOfDay` section in settings.json. + ### EngineSound To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. From d47d4218a44d95540fb9b34cd9736268e1968c0e Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 01:08:43 +0530 Subject: [PATCH 008/121] Switch epoch time functions to system_clock --- AirLib/include/common/ScalableClock.hpp | 2 -- AirLib/include/common/common_utils/Utils.hpp | 13 ++++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/AirLib/include/common/ScalableClock.hpp b/AirLib/include/common/ScalableClock.hpp index 843a768211..7a8b66c682 100644 --- a/AirLib/include/common/ScalableClock.hpp +++ b/AirLib/include/common/ScalableClock.hpp @@ -66,8 +66,6 @@ class ScalableClock : public ClockBase { private: - typedef std::chrono::high_resolution_clock clock; - double scale_; TTimeDelta latency_; double offset_; diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index 561f6ed945..86e4da0862 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -524,15 +524,18 @@ class Utils { } //high precision time in seconds since epoch - static double getTimeSinceEpochSecs(std::chrono::high_resolution_clock::time_point* t = nullptr) + static double getTimeSinceEpochSecs(std::chrono::system_clock::time_point* t = nullptr) { - using Clock = std::chrono::high_resolution_clock; + using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++ return std::chrono::duration((t != nullptr ? *t : Clock::now() ).time_since_epoch()).count(); } - static uint64_t getTimeSinceEpochNanos(std::chrono::high_resolution_clock::time_point* t = nullptr) + static uint64_t getTimeSinceEpochNanos(std::chrono::system_clock::time_point* t = nullptr) { - using Clock = std::chrono::high_resolution_clock; - return static_cast((t != nullptr ? *t : Clock::now() ).time_since_epoch().count()); + using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++ + return std::chrono::duration_cast( + (t != nullptr ? *t : Clock::now()) + .time_since_epoch()). + count(); } template From 890ee12459492bc4bdea36241bcd4c2a653c5f61 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 21:33:47 +0530 Subject: [PATCH 009/121] Minor refactoring for PR https://github.com/Microsoft/AirSim/pull/850 --- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 45 ++++++++++ .../Plugins/AirSim/Source/AirBlueprintLib.h | 5 ++ .../Plugins/AirSim/Source/CameraDirector.cpp | 84 +++++++++---------- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 3 - .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 3 + 5 files changed, 95 insertions(+), 45 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 1a12fecf45..2e0fb244b2 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -17,6 +17,7 @@ #include "Kismet/KismetStringLibrary.h" #include "MessageDialog.h" #include "Engine/LocalPlayer.h" +#include "Slate/SceneViewport.h" #include "Engine/Engine.h" /* @@ -28,6 +29,7 @@ parameters -> camel_case bool UAirBlueprintLib::log_messages_hidden = false; +uint32_t UAirBlueprintLib::FlushOnDrawCount = 0; msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method = msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName; @@ -58,6 +60,49 @@ void UAirBlueprintLib::enableWorldRendering(AActor* context, bool enable) } } +void UAirBlueprintLib::enableViewportRendering(AActor* context, bool enable) +{ + // Enable/disable primary viewport rendering flag + auto* viewport = context->GetWorld()->GetGameViewport(); + if (!viewport) + return; + + if (!enable) { + // This disables rendering of the main viewport in the same way as the + // console command "show rendering" would do. + viewport->EngineShowFlags.SetRendering(false); + + // When getting an image through the API, the image is produced after the render + // thread has finished rendering the current and the subsequent frame. This means + // that the frame rate for obtaining images through the API is only half as high as + // it could be, since only every other image is actually captured. We work around + // this by telling the viewport to flush the rendering queue at the end of each + // drawn frame so that it executes our render request at that point already. + // Do this only if the main viewport is not being rendered anyway in case there are + // any adverse performance effects during main rendering. + //HACK: FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves + if (FlushOnDrawCount == 0) + viewport->GetGameViewport()->IncrementFlushOnDraw(); + } + else { + viewport->EngineShowFlags.SetRendering(true); + + //HACK: FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves + if (FlushOnDrawCount > 0) + viewport->GetGameViewport()->DecrementFlushOnDraw(); + } +} + +void UAirBlueprintLib::OnBeginPlay() +{ + FlushOnDrawCount = 0; +} + +void UAirBlueprintLib::OnEndPlay() +{ + //nothing to do for now +} + void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec) { if (log_messages_hidden) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 88363806cc..070fd1991b 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -34,6 +34,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary GENERATED_BODY() public: + static void OnBeginPlay(); + static void OnEndPlay(); static void LogMessageString(const std::string &prefix, const std::string &suffix, LogDebugLevel level, float persist_sec = 60); UFUNCTION(BlueprintCallable, Category = "Utils") static void LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec = 60); @@ -98,6 +100,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary } static void enableWorldRendering(AActor* context, bool enable); + static void enableViewportRendering(AActor* context, bool enable); private: template @@ -115,6 +118,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary private: static bool log_messages_hidden; + //FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves + static uint32_t FlushOnDrawCount; static msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType mesh_naming_method; }; diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index a56d76881c..756b87e1ef 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -1,6 +1,5 @@ #include "CameraDirector.h" #include "GameFramework/PlayerController.h" -#include "Runtime/Engine/Public/Slate/SceneViewport.h" #include "AirBlueprintLib.h" ACameraDirector::ACameraDirector() @@ -140,52 +139,53 @@ void ACameraDirector::attachSpringArm(bool attach) void ACameraDirector::setMode(ECameraDirectorMode mode) { - //if prev mode was spring arm but new mode isn't then detach spring arm - if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE && - mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE) - { - attachSpringArm(false); - } + { //first remove any settings done by previous mode + + //detach spring arm + if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE && + mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE) + { + attachSpringArm(false); + } - // Enable/disable primary viewport rendering flag - if (mode_ != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY && - mode == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) - { - // This disables rendering of the main viewport in the same way as the - // console command "show rendering" would do. - GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(false); - - // When getting an image through the API, the image is produced after the render - // thread has finished rendering the current and the subsequent frame. This means - // that the frame rate for obtaining images through the API is only half as high as - // it could be, since only every other image is actually captured. We work around - // this by telling the viewport to flush the rendering queue at the end of each - // drawn frame so that it executes our render request at that point already. - // Do this only if the main viewport is not being rendered anyway in case there are - // any adverse performance effects during main rendering. - GetWorld()->GetGameViewport()->GetGameViewport()->IncrementFlushOnDraw(); - } - else if(mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY && - mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) - { // Re-enable rendering - GetWorld()->GetGameViewport()->EngineShowFlags.SetRendering(true); - GetWorld()->GetGameViewport()->GetGameViewport()->DecrementFlushOnDraw(); + if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY && + mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) + { + UAirBlueprintLib::enableViewportRendering(this, true); + } + + //Remove any existing key bindings for manual mode + if (mode != ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL) { + if (external_camera_ != nullptr + && manual_pose_controller_->getActor() == external_camera_) { + + manual_pose_controller_->enableBindings(false); + } + //else someone else is bound to manual pose controller, leave it alone + } } + + { //perform any settings to enter in to this mode + + switch (mode) { + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL: + //if new mode is manual mode then add key bindings + manual_pose_controller_->enableBindings(true); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE: + //if we switched to spring arm mode then attach to spring arm (detachment was done earlier in method) + attachSpringArm(true); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY: + UAirBlueprintLib::enableViewportRendering(this, false); break; + default: + //other modes don't need special setup + break; + } - mode_ = mode; + } - //if new mode is manual mode then add key bindings - if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL) - manual_pose_controller_->enableBindings(true); - //else remove any existing key bindings for manual mode - else if (external_camera_ != nullptr && manual_pose_controller_->getActor() == external_camera_) - manual_pose_controller_->enableBindings(false); - //else someone else is bound to manual pose controller, leave it alone - - //if we switched to spring arm mode then attach to spring arm (detachment was done earlier in method) - if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE) - attachSpringArm(true); + //make switch official + mode_ = mode; } void ACameraDirector::setupInputBindings() diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index d5bdb92566..861344010f 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -135,9 +135,6 @@ void APIPCamera::setImageTypeSettings(int image_type, const APIPCamera::CaptureS setNoiseMaterial(image_type, camera_, camera_->PostProcessSettings, noise_setting); } - - - } void APIPCamera::updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index ccb89f3f91..fb57b4b523 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -21,6 +21,7 @@ void ASimHUD::BeginPlay() Super::BeginPlay(); try { + UAirBlueprintLib::OnBeginPlay(); initializeSettings(); setUnrealEngineSettings(); createSimMode(); @@ -52,6 +53,8 @@ void ASimHUD::EndPlay(const EEndPlayReason::Type EndPlayReason) simmode_ = nullptr; } + UAirBlueprintLib::OnEndPlay(); + Super::EndPlay(EndPlayReason); } From 71550d43c7ecec457e80c2470dcda27bad165969 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 21:46:33 +0530 Subject: [PATCH 010/121] updated doc for ViewMode == NoDisplay --- docs/settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/settings.md b/docs/settings.md index 35ca0b85fc..238bf1ad9f 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -178,6 +178,7 @@ The ViewMode determines how you will view the vehicle. For multirotors, the defa * Fpv: View the scene from front camera of vehicle * Manual: Don't move camera automatically. Use arrow keys and ASWD keys for move camera manually. * SpringArmChase: Chase the vehicle with camera mounted on (invisible) arm that is attached to the vehicle via spring (so it has some latency in movement). +* NoDisplay: This will freeze rendering for main screen however rendering for subwindows, recording and APIs remain active. This mode is useful to save resources in "headless" mode where you are only interested in getting images and don't care about what gets rendered on main screen. This may also improve FPS for recording images. ### TimeOfDay This section of settings controls the position of Sun in the environment. By default `Enabled` is false which means Sun's position is left at whatever was the default in the environment and it doesn't change over the time. If `Enabled` is true then Sun position is computed using longitude, latitude and altitude specified in `OriginGeopoint` section for the date specified in `StartDateTime` in the string format as `%Y-%m-%d %H:%M:%S`, for example, `2018-02-12 15:20:00`. If this string is empty then current date and time is used. If `StartDateTimeDst` is true then we adjust for day light savings time. The Sun's position is then contibuously updated at the interval specified in `UpdateIntervalSecs`. In some cases, it might be desirable to have celestial clock run faster or slower than simulation clock. This can be specified using `CelestialClockSpeed`, for example, value 100 means for every 1 second of simulation clock, Sun's position is advanced by 100 seconds so Sun will move in sky much faster. From 695d8acf51c515628288b369c8812025d7b96a1d Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 22:51:16 +0530 Subject: [PATCH 011/121] immediate stop on reset for cars, https://github.com/Microsoft/AirSim/issues/815 --- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 40 +++++++++++++++++++ .../Plugins/AirSim/Source/AirBlueprintLib.h | 3 ++ .../Plugins/AirSim/Source/Car/CarPawnApi.cpp | 13 +++++- .../AirSim/Source/VehiclePawnWrapper.cpp | 2 +- 4 files changed, 56 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 2e0fb244b2..9003fca4f7 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -60,6 +60,46 @@ void UAirBlueprintLib::enableWorldRendering(AActor* context, bool enable) } } +void UAirBlueprintLib::setSimulatePhysics(AActor* actor, bool simulate_physics) +{ + TInlineComponentArray components; + actor->GetComponents(components); + + for (UPrimitiveComponent* component : components) + { + component->SetSimulatePhysics(simulate_physics); + } +} + +std::vector UAirBlueprintLib::getPhysicsComponents(AActor* actor) +{ + std::vector phys_comps; + TInlineComponentArray components; + actor->GetComponents(components); + + for (UPrimitiveComponent* component : components) + { + if (component->IsSimulatingPhysics()) + phys_comps.push_back(component); + } + + return phys_comps; +} + +void UAirBlueprintLib::resetSimulatePhysics(AActor* actor) +{ + TInlineComponentArray components; + actor->GetComponents(components); + + for (UPrimitiveComponent* component : components) + { + if (component->IsSimulatingPhysics()) { + component->SetSimulatePhysics(false); + component->SetSimulatePhysics(true); + } + } +} + void UAirBlueprintLib::enableViewportRendering(AActor* context, bool enable) { // Enable/disable primary viewport rendering flag diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 070fd1991b..08794aa959 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -101,6 +101,9 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void enableWorldRendering(AActor* context, bool enable); static void enableViewportRendering(AActor* context, bool enable); + static void setSimulatePhysics(AActor* actor, bool simulate_physics); + static void resetSimulatePhysics(AActor* actor); + static std::vector getPhysicsComponents(AActor* actor); private: template diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index 0c2aad7212..de10701c8b 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -114,14 +114,25 @@ CarApiBase::CarState CarPawnApi::getCarState() void CarPawnApi::reset() { last_controls_ = CarControls(); - UAirBlueprintLib::RunCommandOnGameThread([this]() { + auto phys_comps = UAirBlueprintLib::getPhysicsComponents(pawn_->getPawn()); + UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { pawn_->reset(); + for (auto* phys_comp : phys_comps) { + phys_comp->SetPhysicsAngularVelocity(FVector::ZeroVector); + phys_comp->SetPhysicsLinearVelocity(FVector::ZeroVector); + phys_comp->SetSimulatePhysics(false); + } //movement_->ResetMoveState(); //movement_->SetActive(false); //movement_->SetActive(true, true); setCarControls(CarControls()); }, true); + + UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { + for (auto* phys_comp : phys_comps) + phys_comp->SetSimulatePhysics(true); + }, true); } void CarPawnApi::simSetPose(const msr::airlib::Pose& pose, bool ignore_collision) diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index ff25b32185..cdc90a1d2f 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -195,7 +195,7 @@ void VehiclePawnWrapper::reset() //} pawn_->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); - //TODO: delete below + //TODO: refactor below code used for playback //std::ifstream sim_log("C:\\temp\\mavlogs\\circle\\sim_cmd_006_orbit 5 1.txt.pos.txt"); //plot(sim_log, FColor::Purple, Vector3r(0, 0, -3)); //std::ifstream real_log("C:\\temp\\mavlogs\\circle\\real_cmd_006_orbit 5 1.txt.pos.txt"); From 15702df85f64c7143bf0fead18ba4c71fa2e761f Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 2 Mar 2018 23:01:33 +0530 Subject: [PATCH 012/121] make sun movable --- .../Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 1ac5250da9..5884d1dda2 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -79,22 +79,14 @@ void ASimModeBase::setupTimeOfDay() if (sky_spheres.Num() >= 1) { sky_sphere_ = sky_spheres[0]; - //for (TFieldIterator PropIt(sky_sphere_->GetClass()); PropIt; ++PropIt) - //{ - // // We are iterating over all properties of Actor - // UProperty* Property{ *PropIt }; - // FString PropertyName{ GetNameSafe(Property) }; - // std::string MyStdString(TCHAR_TO_UTF8(*PropertyName)); - // Utils::log(MyStdString); - //} static const FName sun_prop_name(TEXT("Directional light actor")); auto* p = sky_sphere_class_->FindPropertyByName(sun_prop_name); UObjectProperty* sun_prop = Cast( p); UObject* sun_obj = sun_prop->GetObjectPropertyValue_InContainer(sky_sphere_); sun_ = Cast(sun_obj); - //if (sun_) { - // sun_->GetRootComponent()->Mobility = EComponentMobility::Movable; - //} + if (sun_) { + sun_->GetRootComponent()->Mobility = EComponentMobility::Movable; + } tod_sim_clock_start_ = ClockFactory::get()->nowNanos(); tod_last_update_ = 0; From 6a68d6bb99ecdc3609ccde7d0eb9f74c4cd0738d Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 5 Mar 2018 03:24:33 +0530 Subject: [PATCH 013/121] fixed https://github.com/Microsoft/AirSim/issues/861 --- AirLib/include/common/AirSimSettings.hpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 4cf277a49a..8fc07c462b 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -397,11 +397,7 @@ struct AirSimSettings { if (json_parent.getChild(child_index, json_settings_child)) { CaptureSetting capture_setting; createCaptureSettings(json_settings_child, capture_setting); - if (capture_setting.image_type >= -1 && capture_setting.image_type < static_cast(capture_settings.size())) - capture_settings[capture_setting.image_type] = capture_setting; - else - //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? - throw std::invalid_argument(std::string("ImageType must be >= -1 and < ") + std::to_string(capture_settings.size())); + capture_settings[capture_setting.image_type] = capture_setting; } } } @@ -441,11 +437,7 @@ struct AirSimSettings { if (json_parent.getChild(child_index, json_settings_child)) { NoiseSetting noise_setting; createNoiseSettings(json_settings_child, noise_setting); - if (noise_setting.ImageType >= -1 && noise_setting.ImageType < static_cast(noise_settings.size())) - noise_settings[noise_setting.ImageType] = noise_setting; - else - //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine? - throw std::invalid_argument("ImageType must be >= -1 and < " + std::to_string(noise_settings.size())); + noise_settings[noise_setting.ImageType] = noise_setting; } } } From 4e8f4f0ec92b059487f64d21ca3dc1280a736d62 Mon Sep 17 00:00:00 2001 From: Manuel Date: Tue, 13 Mar 2018 14:03:51 +0100 Subject: [PATCH 014/121] unzip missing in setup file --- setup.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.sh b/setup.sh index fcd1dc243e..19ea18c499 100755 --- a/setup.sh +++ b/setup.sh @@ -39,6 +39,7 @@ else wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - sudo apt-get update sudo apt-get install -y clang-3.9 clang++-3.9 + sudo apt-get install -y unzip export C_COMPILER=clang-3.9 export COMPILER=clang++-3.9 From 9c2213415c6712f24807f8330eb4df94588eb530 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Tue, 13 Mar 2018 16:37:04 +0100 Subject: [PATCH 015/121] Allow to configure additional cameras on the drone in the JSON settings --- AirLib/include/common/AirSimSettings.hpp | 32 +++++++++++++++++++ .../AirSim/Source/Multirotor/FlyingPawn.cpp | 30 +++++++++++++++-- .../AirSim/Source/Multirotor/FlyingPawn.h | 8 +++-- .../Multirotor/SimModeWorldMultiRotor.cpp | 2 +- 4 files changed, 66 insertions(+), 6 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 8fc07c462b..bb59081890 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -63,6 +63,16 @@ struct AirSimSettings { } }; + struct AdditionalCameraSetting { + // Additional camera positions + float x = 0.5f; + float y = 0.0f; + float z = 0.0f; + float yaw = 0.0f; + float pitch = 0.0f; + float roll = 0.0f; + }; + struct CaptureSetting { //below settinsg are obtained by using Unreal console command (press ~): // ShowFlag.VisualizeHDR 1. @@ -146,6 +156,7 @@ struct AirSimSettings { std::vector subwindow_settings; + std::vector additional_camera_settings; std::map capture_settings; std::map noise_settings; @@ -194,6 +205,7 @@ struct AirSimSettings { loadSubWindowsSettings(settings); loadViewModeSettings(settings); loadRecordingSettings(settings); + loadAdditionalCameraSettings(settings); loadCaptureSettings(settings); loadCameraNoiseSettings(settings); loadSegmentationSettings(settings); @@ -463,6 +475,26 @@ struct AirSimSettings { noise_setting.HorzDistortionContrib = settings.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib); } + void loadAdditionalCameraSettings(const Settings& settings) + { + Settings json_parent; + if (settings.getChild("AdditionalCameras", json_parent)) { + for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) { + Settings additional_camera_setting; + if (json_parent.getChild(child_index, additional_camera_setting)) { + AdditionalCameraSetting setting; + setting.x = additional_camera_setting.getFloat("X", setting.x); + setting.y = additional_camera_setting.getFloat("Y", setting.y); + setting.z = additional_camera_setting.getFloat("Z", setting.z); + setting.yaw = additional_camera_setting.getFloat("Yaw", setting.yaw); + setting.pitch = additional_camera_setting.getFloat("Pitch", setting.pitch); + setting.roll = additional_camera_setting.getFloat("Roll", setting.roll); + additional_camera_settings.push_back(setting); + } + } + } + } + void createCaptureSettings(const msr::airlib::Settings& settings, CaptureSetting& capture_setting) { capture_setting.width = settings.getInt("Width", capture_setting.width); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.cpp index 3e6f946ccd..be9bcea5ac 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.cpp @@ -8,16 +8,23 @@ AFlyingPawn::AFlyingPawn() { + static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); + pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + wrapper_.reset(new VehiclePawnWrapper()); } -void AFlyingPawn::initializeForBeginPlay() +void AFlyingPawn::initializeForBeginPlay(const std::vector& additionalCameras) { //get references of components so we can use later - setupComponentReferences(); + setupComponentReferences(additionalCameras); std::vector cameras = {fpv_camera_front_center_, fpv_camera_front_right_, fpv_camera_front_left_, fpv_camera_bottom_center_, fpv_camera_back_center_}; + for (APIPCamera* camera : AdditionalCameras) { + cameras.push_back(camera); + } + wrapper_->initialize(this, cameras); } @@ -43,7 +50,7 @@ void AFlyingPawn::setRotorSpeed(int rotor_index, float radsPerSec) } } -void AFlyingPawn::setupComponentReferences() +void AFlyingPawn::setupComponentReferences(const std::vector& additionalCameras) { fpv_camera_front_right_ = Cast( (UAirBlueprintLib::GetActorComponent(this, TEXT("FrontRightCamera")))->GetChildActor()); @@ -56,6 +63,23 @@ void AFlyingPawn::setupComponentReferences() fpv_camera_bottom_center_ = Cast( (UAirBlueprintLib::GetActorComponent(this, TEXT("BottomCenterCamera")))->GetChildActor()); + //UStaticMeshComponent* bodyMesh = UAirBlueprintLib::GetActorComponent(this, TEXT("BodyMesh")); + USceneComponent* bodyMesh = GetRootComponent(); + FActorSpawnParameters camera_spawn_params; + camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + // Can't obtain NedTransform from wrapper because it's not initialized yet, so make our own. + NedTransform transform; + transform.initialize(this); + + for (const msr::airlib::AirSimSettings::AdditionalCameraSetting& setting : additionalCameras) { + FVector position = transform.toNeuUU(NedTransform::Vector3r(setting.x, setting.y, setting.z)) - transform.toNeuUU(NedTransform::Vector3r(0.0, 0.0, 0.0)); + FTransform camera_transform(FRotator(setting.pitch, setting.yaw, setting.roll), position, FVector(1., 1., 1.)); + APIPCamera* camera = GetWorld()->SpawnActor(pip_camera_class_, camera_transform, camera_spawn_params); + camera->AttachToComponent(bodyMesh, FAttachmentTransformRules::KeepRelativeTransform); + AdditionalCameras.Add(camera); + } + for (auto i = 0; i < rotor_count; ++i) { rotating_movements_[i] = UAirBlueprintLib::GetActorComponent(this, TEXT("Rotation") + FString::FromInt(i)); } diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h index c33762919f..bf6e777a74 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/FlyingPawn.h @@ -19,15 +19,17 @@ class AIRSIM_API AFlyingPawn : public APawn float RotatorFactor = 1.0f; void setRotorSpeed(int rotor_index, float radsPerSec); - void initializeForBeginPlay(); + void initializeForBeginPlay(const std::vector& additionalCameras); VehiclePawnWrapper* getVehiclePawnWrapper(); virtual void NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) override; private: //methods - void setupComponentReferences(); + void setupComponentReferences(const std::vector& additionalCameras); private: //variables + UPROPERTY() UClass* pip_camera_class_; + //Unreal components static constexpr size_t rotor_count = 4; UPROPERTY() APIPCamera* fpv_camera_front_left_; @@ -38,5 +40,7 @@ class AIRSIM_API AFlyingPawn : public APawn UPROPERTY() URotatingMovementComponent* rotating_movements_[rotor_count]; + UPROPERTY() TArray AdditionalCameras; + std::unique_ptr wrapper_; }; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index 2e15fd1506..bef8c14812 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -117,7 +117,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve { //initialize each vehicle pawn we found TMultiRotorPawn* vehicle_pawn = static_cast(pawn); - vehicle_pawn->initializeForBeginPlay(); + vehicle_pawn->initializeForBeginPlay(getSettings().additional_camera_settings); //chose first pawn as FPV if none is designated as FPV VehiclePawnWrapper* wrapper = vehicle_pawn->getVehiclePawnWrapper(); From 164d0ead81b07c8e9a503aeb7fc4d940878972f6 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Tue, 13 Mar 2018 16:55:33 +0100 Subject: [PATCH 016/121] Add documentation for additional cameras --- docs/settings.md | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/docs/settings.md b/docs/settings.md index 238bf1ad9f..972294eb77 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -116,6 +116,9 @@ Below are complete list of settings available along with their default values. I "HorzDistortionStrength": 0.002 } ], + "AdditionalCameras": [ + { "X": 0.00, "Y": 0.5, "Z": 0.0, "Roll": 0.0, "Pitch": 0.0, "Yaw": 90.0 } + ], "PX4": { "FirmwareName": "PX4", "LogViewerHostIp": "127.0.0.1", @@ -253,4 +256,11 @@ This adds regions of noise on horizontal lines. #### Horizontal line distortion This adds fluctuations on horizontal line. * `HorzDistortionContrib`: This determines blend ratio of noise pixel with image pixel, 0 means no noise and 1 means only noise. -* `HorzDistortionStrength`: This determines how large is the distortion. \ No newline at end of file +* `HorzDistortionStrength`: This determines how large is the distortion. + +### Additional Camera Settings +This allows to configure cameras in addition to the [standard ones](image_apis.md#available-cameras). This is only implemented in the multirotor drone at the moment. +The X, Y and Z fields specify the location of the new camera in the body frame, where X points forward, Y points to the right, and Z points downwards, and the values are given +in SI units (meters). Yaw, Pitch, and Roll specify the orientation of the camera, where Yaw denotes rotation around the Z axis, Pitch rotation around the Y axis and Roll rotation around the X axis. + +This particular example adds a camera that is mounted on the right side of the drone, pointed to the right. From 4e124bcaad6fa00cf2850760369f5ae9b22bb696 Mon Sep 17 00:00:00 2001 From: Armin Burgmeier Date: Tue, 13 Mar 2018 17:11:25 +0100 Subject: [PATCH 017/121] Add a note about camera indices --- docs/settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/settings.md b/docs/settings.md index 972294eb77..6ce37adb2c 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -263,4 +263,4 @@ This allows to configure cameras in addition to the [standard ones](image_apis.m The X, Y and Z fields specify the location of the new camera in the body frame, where X points forward, Y points to the right, and Z points downwards, and the values are given in SI units (meters). Yaw, Pitch, and Roll specify the orientation of the camera, where Yaw denotes rotation around the Z axis, Pitch rotation around the Y axis and Roll rotation around the X axis. -This particular example adds a camera that is mounted on the right side of the drone, pointed to the right. +This particular example adds a camera that is mounted on the right side of the drone, pointed to the right. The camera indices of the additional cameras are subsequent to the default ones, so camera index 5 is the first additional camera, camera index 6 the second additional camera, and so on. From 349841c1ec5e5f213160dcc68b81001b72279a33 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 14 Mar 2018 16:36:15 -0700 Subject: [PATCH 018/121] fixed car controls in log: https://github.com/Microsoft/AirSim/issues/869 --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 25 ++++++++++---------- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 1 + 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 94a6412211..1cfbe5189b 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -414,7 +414,6 @@ void ACarPawn::Tick(float Delta) void ACarPawn::updateCarControls() { - const msr::airlib::CarApiBase::CarControls* current_controls = nullptr; if (wrapper_->getRemoteControlID() >= 0 && joystick_state_.is_initialized) { joystick_.getJoyStickState(0, joystick_state_); @@ -442,25 +441,25 @@ void ACarPawn::updateCarControls() } UAirBlueprintLib::LogMessageString("Control Mode: ", "Wheel/Joystick", LogDebugLevel::Informational); - current_controls = &joystick_controls_; + current_controls_ = joystick_controls_; } else { UAirBlueprintLib::LogMessageString("Control Mode: ", "Keyboard", LogDebugLevel::Informational); - current_controls = &keyboard_controls_; + current_controls_ = keyboard_controls_; } if (!api_->isApiControlEnabled()) { - api_->setCarControls(* current_controls); + api_->setCarControls(current_controls_); } else { UAirBlueprintLib::LogMessageString("Control Mode: ", "API", LogDebugLevel::Informational); - current_controls = & api_->getCarControls(); + current_controls_ = api_->getCarControls(); } - UAirBlueprintLib::LogMessageString("Accel: ", std::to_string(current_controls->throttle), LogDebugLevel::Informational); - UAirBlueprintLib::LogMessageString("Break: ", std::to_string(current_controls->brake), LogDebugLevel::Informational); - UAirBlueprintLib::LogMessageString("Steering: ", std::to_string(current_controls->steering), LogDebugLevel::Informational); - UAirBlueprintLib::LogMessageString("Handbreak: ", std::to_string(current_controls->handbrake), LogDebugLevel::Informational); - UAirBlueprintLib::LogMessageString("Target Gear: ", std::to_string(current_controls->manual_gear), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessageString("Accel: ", std::to_string(current_controls_.throttle), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessageString("Break: ", std::to_string(current_controls_.brake), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessageString("Steering: ", std::to_string(current_controls_.steering), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessageString("Handbreak: ", std::to_string(current_controls_.handbrake), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessageString("Target Gear: ", std::to_string(current_controls_.manual_gear), LogDebugLevel::Informational); } void ACarPawn::BeginPlay() @@ -552,9 +551,9 @@ std::string ACarPawn::getLogString() std::string logString = std::to_string(timestamp_millis).append("\t") .append(std::to_string(KPH_int).append("\t")) - .append(std::to_string(keyboard_controls_.throttle)).append("\t") - .append(std::to_string(keyboard_controls_.steering)).append("\t") - .append(std::to_string(keyboard_controls_.brake)).append("\t") + .append(std::to_string(current_controls_.throttle)).append("\t") + .append(std::to_string(current_controls_.steering)).append("\t") + .append(std::to_string(current_controls_.brake)).append("\t") .append(gear).append("\t"); return logString; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 170478d075..06f9001637 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -160,6 +160,7 @@ class ACarPawn : public AWheeledVehicle CarPawnApi::CarControls keyboard_controls_; CarPawnApi::CarControls joystick_controls_; + CarPawnApi::CarControls current_controls_; SimJoyStick joystick_; SimJoyStick::State joystick_state_; From e6606751b8276a86205db2fdf58ea761741f1a09 Mon Sep 17 00:00:00 2001 From: Joerg Reichardt Date: Wed, 21 Mar 2018 14:32:23 +0100 Subject: [PATCH 019/121] Fixup so it compiles in Unreal 4.19 --- Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp | 2 +- .../Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 4afe7cbc6f..4792739589 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -54,7 +54,7 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) { //get player controller APlayerController* player_controller = this->GetWorld()->GetFirstPlayerController(); - FTransform actor_transform = player_controller->GetActorTransform(); + FTransform actor_transform = player_controller->GetViewTarget()->GetActorTransform(); //put camera little bit above vehicle FTransform camera_transform(actor_transform.GetLocation() + FVector(follow_distance_, 0, 400)); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index bef8c14812..a75a8250ac 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -62,7 +62,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve { //get player controller APlayerController* player_controller = this->GetWorld()->GetFirstPlayerController(); - FTransform actor_transform = player_controller->GetActorTransform(); + FTransform actor_transform = player_controller->GetViewTarget()->GetActorTransform(); //put camera little bit above vehicle FTransform camera_transform(actor_transform.GetLocation() + FVector(-300, 0, 200)); From 7983eff0c76795e884a5a2a0a4731829b8a874c0 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 21 Mar 2018 18:29:20 -0700 Subject: [PATCH 020/121] Add KML support for viewing gx:Track or LineString data. --- LogViewer/LogViewer/App.config | 6 +- LogViewer/LogViewer/LogViewer.csproj | 4 +- LogViewer/LogViewer/MainWindow.xaml.cs | 69 +++++- LogViewer/LogViewer/Model/KmlDataLog.cs | 232 ++++++++++++++++++ LogViewer/LogViewer/Model/Px4DataLog.cs | 41 +++- .../Properties/Resources.Designer.cs | 44 ++-- .../LogViewer/Properties/Settings.Designer.cs | 22 +- LogViewer/Networking/Networking.csproj | 3 +- 8 files changed, 367 insertions(+), 54 deletions(-) create mode 100644 LogViewer/LogViewer/Model/KmlDataLog.cs diff --git a/LogViewer/LogViewer/App.config b/LogViewer/LogViewer/App.config index d740e88600..45437954e0 100644 --- a/LogViewer/LogViewer/App.config +++ b/LogViewer/LogViewer/App.config @@ -1,6 +1,6 @@ - + - + - \ No newline at end of file + diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 2b307707de..2a2ad8f4b6 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -9,7 +9,7 @@ Properties LogViewer Px4LogViewer - v4.5.2 + v4.6.1 512 {60dc8134-eba5-43b8-bcc9-bb4bc16c2548};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} 4 @@ -37,6 +37,7 @@ false true true + AnyCPU @@ -141,6 +142,7 @@ + diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 3a09a0b787..8c4a8c2f10 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -334,7 +334,7 @@ private async void OnOpenFile(object sender, RoutedEventArgs e) OpenButton.IsEnabled = false; Microsoft.Win32.OpenFileDialog fo = new Microsoft.Win32.OpenFileDialog(); - fo.Filter = "PX4 Log Files (*.px4log)|*.px4log|PX4 ulog Files (*.ulg)|*.ulg|CSV Files (*.csv)|*.csv|bin files (*.bin)|*.bin|mavlink files (*.mavlink)|*.mavlink|JSON files (*.json)|*.json"; + fo.Filter = "PX4 Log Files (*.px4log)|*.px4log|PX4 ulog Files (*.ulg)|*.ulg|CSV Files (*.csv)|*.csv|bin files (*.bin)|*.bin|mavlink files (*.mavlink)|*.mavlink|JSON files (*.json)|*.json|KML files (*.kml)|*.kml"; fo.CheckFileExists = true; fo.Multiselect = true; if (fo.ShowDialog() == true) @@ -361,6 +361,9 @@ private async void OnOpenFile(object sender, RoutedEventArgs e) case ".json": await Task.Run(async () => { await LoadJSonFile(file); }); break; + case ".kml": + await Task.Run(async () => { await LoadKmlFile(file); }); + break; default: MessageBox.Show("Do not know how to read files of type : " + System.IO.Path.GetExtension(file), "Unsupported file extension", MessageBoxButton.OK, MessageBoxImage.Exclamation); @@ -419,6 +422,35 @@ private async Task LoadJSonFile(string file) UpdateButtons(); } + private async Task LoadKmlFile(string file) + { + try + { + await Dispatcher.BeginInvoke(new Action(() => + { + SystemConsole.Show(); + })).Task; + + AppendMessage("Loading " + file); + ShowStatus("Loading " + file); + + XDocument doc = XDocument.Load(file); + KmlDataLog data = new Model.KmlDataLog(); + data.Load(doc); + + ShowSchema(); + + LoadFlights(data); + this.logs.Add(data); + } + catch (Exception ex) + { + AppendMessage("### Error loading KML file: " + ex.Message); + } + ShowStatus("Done Loading " + file); + UpdateButtons(); + } + private async Task LoadULogFile(string file) { try @@ -805,8 +837,14 @@ void ShowMap() //Debug.WriteLine("{0},\t{1},\t{2},\t{3},\t\t{4:F2},\t{5},\t{6}", gps.GPSTime, gps.Lat, gps.Lon, gps.nSat, gps.Alt, gps.EPH, gps.Fix); if (!(Math.Floor(gps.Lat) == 0 && Math.Floor(gps.Lon) == 0)) { + // map doesn't like negative altitudes. + double alt = gps.Alt; + if (alt < 0) + { + alt = 0; + } mapData.Add(gps); - var pos = new Location() { Altitude = gps.Alt, Latitude = gps.Lat, Longitude = gps.Lon }; + var pos = new Location() { Altitude = alt, Latitude = gps.Lat, Longitude = gps.Lon }; points.Add(pos); ulong time = (ulong)gps.GPSTime; if (time != 0) @@ -857,7 +895,7 @@ void ShowMap() { try { - myMap.SetView(last.Locations, new Thickness(20.0), 0); + myMap.SetView(GetBoundingBox(last.Locations), new Thickness(20.0), 0); } catch (Exception ex) { @@ -867,6 +905,31 @@ void ShowMap() } + private LocationCollection GetBoundingBox(LocationCollection locations) + { + if (locations.Count == 0) + { + throw new Exception("Must provide at least one location"); + } + Location first = locations.First(); + double minLat = first.Latitude; + double maxLat = first.Latitude; + double minlong = first.Longitude; + double maxLong = first.Longitude; + foreach (Location i in locations) + { + minLat = Math.Min(minLat, i.Latitude); + maxLat = Math.Max(maxLat, i.Latitude); + minlong = Math.Min(minlong, i.Longitude); + maxLong = Math.Max(maxLong, i.Longitude); + } + var corners = new LocationCollection(); + corners.Add(new Location(minLat, minlong)); + corners.Add(new Location(minLat, minlong )); + corners.Add(new Location(minLat, minlong )); + corners.Add(new Location(minLat, minlong )); + return corners; + } private void UpdateButtons() { diff --git a/LogViewer/LogViewer/Model/KmlDataLog.cs b/LogViewer/LogViewer/Model/KmlDataLog.cs new file mode 100644 index 0000000000..4116d3169c --- /dev/null +++ b/LogViewer/LogViewer/Model/KmlDataLog.cs @@ -0,0 +1,232 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using System.Xml.Linq; + +namespace LogViewer.Model +{ + class KmlDataLog : IDataLog + { + DateTime startTime; + DateTime endTime; + XNamespace kmlns = XNamespace.Get("http://www.opengis.net/kml/2.2"); + XNamespace gxns = XNamespace.Get("http://www.google.com/kml/ext/2.2"); + LogItemSchema schema; + List flights = new List(); + List log = new List(); + + public KmlDataLog() + { + startTime = DateTime.Now; + endTime = startTime; + } + + public void Load(XDocument doc) + { + var root = doc.Root; + if (root.Name.LocalName != "kml" || root.Name.Namespace != kmlns) + { + throw new Exception("Doesn't appear to be KML file, expecting XML namespace '" + kmlns + "'"); + } + + Dictionary styles = new Dictionary(); + + var document = root.Element(kmlns + "Document"); + if (document != null) { + foreach (XElement style in document.Elements(kmlns + "Style")) + { + string id = (string)style.Attribute("id"); + if (!string.IsNullOrEmpty(id)) + { + styles[id] = style; + } + } + foreach (XElement placemark in document.Elements(kmlns + "Placemark")) + { + string name = (string)document.Element(kmlns + "name"); + string styleUrl = (string)document.Element(kmlns + "styleUrl"); + bool started = false; + DateTime current = DateTime.Now; + DateTime startTime = current; + DateTime endTime = startTime; + + // "Track" data is one style of KML that we can load GPS data from + foreach (XElement track in placemark.Elements(gxns + "Track")) + { + foreach (XElement child in track.Elements()) + { + switch(child.Name.LocalName) + { + case "altitudeMode": + // absolute? + break; + case "interpolate": + // 0? + break; + case "when": + var time = DateTime.Parse((string)child); + if (!started) + { + startTime = time; + started = true; + } + current = time; + endTime = time; + break; + case "coord": + AddGpsData(current, startTime, (string)child, ' '); + break; + } + } + } + + // "LineString" is another format. + foreach (XElement ls in placemark.Elements(kmlns + "LineString")) + { + if (!started) + { + started = true; + } + + XElement coordinates = ls.Element(kmlns + "coordinates"); + foreach (string line in ((string)coordinates).Split('\n')) + { + // fake time, since this format has no time. + current = current + TimeSpan.FromMilliseconds(1); + endTime = current; + AddGpsData(current, startTime, line, ','); + } + } + + + if (started) + { + this.flights.Add(new Flight() + { + Duration = endTime - startTime, + Log = this, + Name = name, + StartTime = startTime + }); + } + } + } + + + } + + private void AddGpsData(DateTime current, DateTime startTime, string row, char separator) + { + string[] parts = row.Split(separator); + if (parts.Length == 3) + { + double lat, lng, alt; + double.TryParse(parts[0], out lat); + double.TryParse(parts[1], out lng); + double.TryParse(parts[2], out alt); + + var gps = new LogEntry() + { + Name = "GPS", + Timestamp = (ulong)current.Ticks + }; + gps.SetField("TimeMS", (UInt32)((current - startTime).Milliseconds)); + gps.SetField("Lat", lat); + gps.SetField("Lng", lng); + gps.SetField("Alt", alt); + log.Add(gps); + } + } + + public TimeSpan Duration + { + get { return endTime - startTime; } + } + + private void Schematize(LogItemSchema schema, IEnumerable items) + { + foreach (var item in items) + { + // is this a new item we haven't seen before? + if (!(from c in schema.ChildItems where c.Name == item.Name select c).Any()) + { + var typeName = item.Name; + if (item is LogEntry) + { + // recurrse down the structure + var s = new LogItemSchema() { Name = item.Name, Type = typeName, ChildItems = new List(), Parent = schema }; + schema.ChildItems.Add(s); + Schematize(s, ((LogEntry)item).GetFields()); + } + else + { + // leaf node + typeName = item.Value.GetType().Name; + var leaf = new LogItemSchema() { Name = item.Name, Type = typeName, Parent = schema }; + schema.ChildItems.Add(leaf); + } + } + } + } + + public LogItemSchema Schema + { + get { + if (this.schema == null) + { + this.schema = new LogItemSchema() { Name = "KmlLog", Type = "Root", ChildItems = new List() }; + Schematize(this.schema, this.log); + } + return this.schema; + } + } + + public DateTime StartTime + { + get { return this.startTime; } + } + + public IEnumerable GetDataValues(LogItemSchema schema, DateTime startTime, TimeSpan duration) + { + if (this.log != null && schema.Parent != null) + { + foreach (LogEntry entry in GetRows(schema.Parent.Name, startTime, duration)) + { + yield return entry.GetDataValue(schema.Name); + } + } + } + + public IEnumerable GetFlights() + { + return this.flights; + } + + public IEnumerable GetRows(string typeName, DateTime startTime, TimeSpan duration) + { + foreach (var row in this.log) + { + if (string.Compare(row.Name, typeName, StringComparison.OrdinalIgnoreCase) == 0 && + (duration == TimeSpan.MaxValue || + row.Timestamp >= (ulong)startTime.Ticks && row.Timestamp < (ulong)(startTime + duration).Ticks)) + { + yield return row; + } + } + } + + public DateTime GetTime(ulong timeMs) + { + TimeSpan ms = TimeSpan.FromMilliseconds(timeMs); + return this.startTime + ms; + } + + public IEnumerable LiveQuery(LogItemSchema schema, CancellationToken token) + { + throw new NotImplementedException(); + } + } +} diff --git a/LogViewer/LogViewer/Model/Px4DataLog.cs b/LogViewer/LogViewer/Model/Px4DataLog.cs index 713c21d754..bb4db3da9e 100644 --- a/LogViewer/LogViewer/Model/Px4DataLog.cs +++ b/LogViewer/LogViewer/Model/Px4DataLog.cs @@ -13,18 +13,16 @@ namespace LogViewer.Model { - class LogField + public class LogField { public string Name { get; set; } public object Value { get; set; } } - public class LogEntry + public class LogEntry : LogField { public ulong Timestamp { get; set; } - - public string Name { get; set; } - + /// /// Return a data value, and try and timestamp in the x-coordinate /// @@ -44,7 +42,7 @@ public virtual DataValue GetDataValue(string field) return null; } if (result != null) - { + { DataValue dv = new DataValue() { X = Timestamp, Y = 0 }; try { @@ -153,6 +151,35 @@ enum PX4_NAV_STATE NAVIGATION_STATE_MAX = 20 } + public bool HasFields + { + get + { + if (cache == null) + { + cache = new Dictionary(); + // crack the blob + ParseFields(); + } + return this.cache != null; + } + } + + public IEnumerable GetFields() + { + if (cache == null) + { + cache = new Dictionary(); + // crack the blob + ParseFields(); + } + if (this.cache != null) + { + return this.cache.Values; + } + return null; + } + public bool HasField(string name) { if (cache == null) @@ -593,7 +620,7 @@ public IEnumerable GetFlights() float max = float.MinValue; foreach (var row in this.data) - { + { if (row.Name == "OUT0") { DataValue f = row.GetDataValue("Out0"); diff --git a/LogViewer/LogViewer/Properties/Resources.Designer.cs b/LogViewer/LogViewer/Properties/Resources.Designer.cs index 8d84fe1243..a659e12c90 100644 --- a/LogViewer/LogViewer/Properties/Resources.Designer.cs +++ b/LogViewer/LogViewer/Properties/Resources.Designer.cs @@ -8,10 +8,10 @@ // //------------------------------------------------------------------------------ -namespace LogViewer.Properties -{ - - +namespace LogViewer.Properties { + using System; + + /// /// A strongly-typed resource class, for looking up localized strings, etc. /// @@ -19,51 +19,43 @@ namespace LogViewer.Properties // class via a tool like ResGen or Visual Studio. // To add or remove a member, edit your .ResX file then rerun ResGen // with the /str option, or rebuild your VS project. - [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "4.0.0.0")] + [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "15.0.0.0")] [global::System.Diagnostics.DebuggerNonUserCodeAttribute()] [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()] - internal class Resources - { - + internal class Resources { + private static global::System.Resources.ResourceManager resourceMan; - + private static global::System.Globalization.CultureInfo resourceCulture; - + [global::System.Diagnostics.CodeAnalysis.SuppressMessageAttribute("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")] - internal Resources() - { + internal Resources() { } - + /// /// Returns the cached ResourceManager instance used by this class. /// [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)] - internal static global::System.Resources.ResourceManager ResourceManager - { - get - { - if ((resourceMan == null)) - { + internal static global::System.Resources.ResourceManager ResourceManager { + get { + if (object.ReferenceEquals(resourceMan, null)) { global::System.Resources.ResourceManager temp = new global::System.Resources.ResourceManager("LogViewer.Properties.Resources", typeof(Resources).Assembly); resourceMan = temp; } return resourceMan; } } - + /// /// Overrides the current thread's CurrentUICulture property for all /// resource lookups using this strongly typed resource class. /// [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)] - internal static global::System.Globalization.CultureInfo Culture - { - get - { + internal static global::System.Globalization.CultureInfo Culture { + get { return resourceCulture; } - set - { + set { resourceCulture = value; } } diff --git a/LogViewer/LogViewer/Properties/Settings.Designer.cs b/LogViewer/LogViewer/Properties/Settings.Designer.cs index caa160bb1a..ebfd3a3bb3 100644 --- a/LogViewer/LogViewer/Properties/Settings.Designer.cs +++ b/LogViewer/LogViewer/Properties/Settings.Designer.cs @@ -8,21 +8,17 @@ // //------------------------------------------------------------------------------ -namespace LogViewer.Properties -{ - - +namespace LogViewer.Properties { + + [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()] - [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "11.0.0.0")] - internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase - { - + [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "15.5.0.0")] + internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase { + private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings()))); - - public static Settings Default - { - get - { + + public static Settings Default { + get { return defaultInstance; } } diff --git a/LogViewer/Networking/Networking.csproj b/LogViewer/Networking/Networking.csproj index 48a4d99c65..65f296f196 100644 --- a/LogViewer/Networking/Networking.csproj +++ b/LogViewer/Networking/Networking.csproj @@ -9,8 +9,9 @@ Properties Microsoft.Networking Networking - v4.5.2 + v4.6.1 512 + true From 39934fad6e7eee1bff20e69b84c62add03605c59 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 21 Mar 2018 18:36:04 -0700 Subject: [PATCH 021/121] Fix bug --- LogViewer/LogViewer/MainWindow.xaml.cs | 2 +- LogViewer/LogViewer/Model/KmlDataLog.cs | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 8c4a8c2f10..1f1473805b 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -895,7 +895,7 @@ void ShowMap() { try { - myMap.SetView(GetBoundingBox(last.Locations), new Thickness(20.0), 0); + myMap.SetView(last.Locations, new Thickness(20.0), 0); } catch (Exception ex) { diff --git a/LogViewer/LogViewer/Model/KmlDataLog.cs b/LogViewer/LogViewer/Model/KmlDataLog.cs index 4116d3169c..a31a995115 100644 --- a/LogViewer/LogViewer/Model/KmlDataLog.cs +++ b/LogViewer/LogViewer/Model/KmlDataLog.cs @@ -77,7 +77,7 @@ public void Load(XDocument doc) endTime = time; break; case "coord": - AddGpsData(current, startTime, (string)child, ' '); + AddGpsData(current, startTime, (string)child, ' ', "OrderLatLong"); break; } } @@ -97,7 +97,7 @@ public void Load(XDocument doc) // fake time, since this format has no time. current = current + TimeSpan.FromMilliseconds(1); endTime = current; - AddGpsData(current, startTime, line, ','); + AddGpsData(current, startTime, line, ',', "OrderLongLat"); } } @@ -118,7 +118,7 @@ public void Load(XDocument doc) } - private void AddGpsData(DateTime current, DateTime startTime, string row, char separator) + private void AddGpsData(DateTime current, DateTime startTime, string row, char separator, string order) { string[] parts = row.Split(separator); if (parts.Length == 3) @@ -128,13 +128,21 @@ private void AddGpsData(DateTime current, DateTime startTime, string row, char s double.TryParse(parts[1], out lng); double.TryParse(parts[2], out alt); + if (order == "OrderLongLat") + { + // switch them. + double temp = lng; + lng = lat; + lat = temp; + } + var gps = new LogEntry() { Name = "GPS", Timestamp = (ulong)current.Ticks }; gps.SetField("TimeMS", (UInt32)((current - startTime).Milliseconds)); - gps.SetField("Lat", lat); + gps.SetField("Lat", lat); gps.SetField("Lng", lng); gps.SetField("Alt", alt); log.Add(gps); From 5b14568ae541fcd18d51548a305425eb163ba084 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 21 Mar 2018 18:37:40 -0700 Subject: [PATCH 022/121] publish --- LogViewer/LogViewer/LogViewer.csproj | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 2a2ad8f4b6..3631e16d1c 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -15,6 +15,7 @@ 4 true true + ftp://www.lovettsoftware.com/LovettSoftware/Downloads/Px4LogViewer/ true Web @@ -32,12 +33,11 @@ PX4 Log Viewer true publish.htm - 43 + 44 1.0.0.%2a false true true - AnyCPU From 931ffd4bb52e3fae468796d9ff674660462967c9 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 21 Mar 2018 21:00:36 -0700 Subject: [PATCH 023/121] Fixes --- LogViewer/LogViewer/MainWindow.xaml.cs | 9 +- LogViewer/LogViewer/Model/CsvDataLog.cs | 216 +++++++++++++-------- LogViewer/LogViewer/Model/KmlDataLog.cs | 6 +- LogViewer/LogViewer/Model/Px4DataLog.cs | 17 +- LogViewer/LogViewer/Utilities/Generated.cs | 12 +- 5 files changed, 179 insertions(+), 81 deletions(-) diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 1f1473805b..2296b7b124 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -1102,7 +1102,14 @@ private void GraphItem(LogItemSchema schema) StringBuilder sb = new StringBuilder(); foreach (var value in GetSelectedDataValues(schema)) { - sb.AppendLine(value.Label); + if (!string.IsNullOrEmpty(value.Label)) + { + sb.AppendLine(value.Label); + } + else + { + sb.AppendLine(value.Y.ToString()); + } } SystemConsole.Write(sb.ToString()); diff --git a/LogViewer/LogViewer/Model/CsvDataLog.cs b/LogViewer/LogViewer/Model/CsvDataLog.cs index e471fad195..9e232eddd6 100644 --- a/LogViewer/LogViewer/Model/CsvDataLog.cs +++ b/LogViewer/LogViewer/Model/CsvDataLog.cs @@ -15,10 +15,11 @@ class CsvDataLog : IDataLog { DateTime startTime = DateTime.MinValue; TimeSpan duration = TimeSpan.Zero; - XDocument data; - LogItemSchema schema = new LogItemSchema() { Name = "CsvDataLog", Type = "Root" }; + LogItemSchema schema; List flights = new List(); string timeElementName; + List log = new List(); + public DateTime StartTime { get { return startTime; } } public TimeSpan Duration { get { return duration; } } @@ -27,46 +28,22 @@ class CsvDataLog : IDataLog public IEnumerable GetDataValues(LogItemSchema schema, DateTime startTime, TimeSpan duration) { - if (data != null) + if (this.log != null && schema.Parent != null) { - bool allValues = (startTime == DateTime.MinValue && duration == TimeSpan.MaxValue); - DateTime endTime = allValues ? DateTime.MaxValue : startTime + duration; - string xname = XmlConvert.EncodeLocalName(schema.Name); - long defaultX = 0; - foreach (var row in data.Root.Elements("row")) + foreach (LogEntry entry in GetRows(schema.Parent.Name, startTime, duration)) { - foreach (var e in row.Elements(xname)) + var dv = entry.GetDataValue(schema.Name); + if (dv != null) { - long? us = GetTimeMicroseconds(row); - long x = us.HasValue ? us.Value : defaultX++; - DateTime time = this.startTime.AddMilliseconds(x / 1000); - if (allValues || (us.HasValue && time > startTime && time < endTime)) - { - string value = e.Value; - double d = 0; - if (double.TryParse(value, out d)) - { - yield return new DataValue() { X = x, Y = d }; - } - } + yield return dv; } } } } - public IEnumerable LiveQuery(LogItemSchema schema, CancellationToken token) - { - throw new NotImplementedException("LiveQuery"); - } - - public IEnumerable GetRows(string typeName, DateTime startTime, TimeSpan duration) - { - throw new NotImplementedException("GetRows"); - } - public DateTime GetTime(ulong timeMs) { - throw new NotImplementedException("GetTime"); + return this.startTime + TimeSpan.FromMilliseconds(timeMs); } public async Task Load(string fileName, ProgressUtility progress) @@ -75,56 +52,130 @@ public async Task Load(string fileName, ProgressUtility progress) // CSV doesn't have realtime clock, so go with the file date instead. this.startTime = File.GetLastWriteTime(fileName); - // time (us) + // time (ms) long min = long.MaxValue; long max = long.MinValue; - await Task.Run(() => { timeElementName = null; using (Stream s = File.OpenRead(fileName)) { + Dictionary map = new Dictionary(); XmlNameTable nametable = new NameTable(); using (XmlCsvReader reader = new XmlCsvReader(s, System.Text.Encoding.UTF8, new Uri(fileName), nametable)) { - progress.ShowProgress(0, s.Length, s.Position); reader.FirstRowHasColumnNames = true; - data = XDocument.Load(reader); - - this.schema = new LogItemSchema() { Name = "CsvLog", Type = "Root" }; - var item = new LogItemSchema() { Name = System.IO.Path.GetFileName(fileName), Type = "Item" }; - - // create the schema - List children = new List(); - foreach (String name in reader.ColumnNames) + reader.ColumnsAsAttributes = true; + while (reader.Read()) { - if (timeElementName == null && name.Contains("time")) + progress.ShowProgress(0, s.Length, s.Position); + if (this.schema == null) { - timeElementName = name; + // create the schema + this.schema = new LogItemSchema() { Name = "CsvLog", Type = "Root" }; + List children = new List(); + LogItemSchema row = null; + + foreach (String name in reader.ColumnNames) + { + if (timeElementName == null && (name.ToLower().Contains("time") || name.ToLower().Contains("ticks"))) + { + timeElementName = name; + } + + if (name.Contains(":")) + { + // then we have sub-parts. + int pos = name.IndexOf(":"); + string key = name.Substring(0, pos); + string field = name.Substring(pos + 1); + LogItemSchema group = null; + if (!map.ContainsKey(key)) + { + group = new LogItemSchema() { Name = key, Parent = this.schema, Type = key, ChildItems = new List() }; + children.Add(group); + map[key] = group; + } + else + { + group = map[key]; + } + var leaf = new LogItemSchema() { Name = field, Parent = group, Type = "Double" }; + group.ChildItems.Add(leaf); + map[name] = leaf; + } + else + { + if (row == null) + { + row = new LogItemSchema() { Name = "Other", Parent = this.schema, Type = "Other", ChildItems = new List() }; + children.Add(row); + } + var leaf = new LogItemSchema() { Name = name, Parent = row, Type = "Double" }; + row.ChildItems.Add(leaf); + map[name] = leaf; + } + } + this.schema.ChildItems = children; } - children.Add(new LogItemSchema() { Name = name, Parent = this.schema, Type = "Double" }); - } - - item.ChildItems = children; - this.schema.ChildItems = new List(new LogItemSchema[] { item }); - - progress.ShowProgress(0, s.Length, s.Position); - } - } - foreach (var e in data.Root.Elements()) - { - long? i = GetTimeMicroseconds(e); - if (i.HasValue) - { - if (i.Value < min) - { - min = i.Value; - } - if (i > max) - { - max = i.Value; + if (reader.NodeType == XmlNodeType.Element && reader.LocalName == "row") + { + // read a row + long time = GetTicks(reader); + min = Math.Min(min, time); + max = Math.Max(max, time); + LogEntry row = new Model.LogEntry() { Name = "Other", Timestamp = (ulong)time }; + log.Add(row); + Dictionary groups = new Dictionary(); + + if (reader.MoveToFirstAttribute()) + { + do + { + string name = XmlConvert.DecodeName(reader.LocalName); + LogItemSchema itemSchema = map[name]; + LogEntry e = row; + if (name.Contains(":")) + { + // then we have sub-parts. + int pos = name.IndexOf(":"); + string key = name.Substring(0, pos); + string field = name.Substring(pos + 1); + if (!groups.ContainsKey(key)) + { + e = new LogEntry() { Name = key, Timestamp = (ulong)time }; + groups[key] = e; + log.Add(e); + } + else + { + e = groups[key]; + } + name = field; + } + + string value = reader.Value; + double d = 0; + if (double.TryParse(value, out d)) + { + e.SetField(name, d); + } + else + { + if (!string.IsNullOrEmpty(value)) + { + // not a number. + itemSchema.Type = "String"; + e.SetField(name, value); + } + } + } + while (reader.MoveToNextAttribute()); + reader.MoveToElement(); + } + } } } } @@ -138,23 +189,36 @@ await Task.Run(() => } - long? GetTimeMicroseconds(XElement e) + long GetTicks(XmlReader e) { - string time = (string)e.Attribute(timeElementName); - if (time == null) + string time = e.GetAttribute(timeElementName); + if (!string.IsNullOrEmpty(time)) { - time = (string)e.Element(timeElementName); + double i = 0; + if (double.TryParse(time, out i)) + { + return (long)i; + } } - double i = 0; - if (double.TryParse(time, out i)) + return 0; + } + + public IEnumerable LiveQuery(LogItemSchema schema, CancellationToken token) + { + throw new NotImplementedException("LiveQuery"); + } + + public IEnumerable GetRows(string typeName, DateTime startTime, TimeSpan duration) + { + foreach (var row in this.log) { - if (timeElementName == "time") + if (string.Compare(row.Name, typeName, StringComparison.OrdinalIgnoreCase) == 0 && + (duration == TimeSpan.MaxValue || + row.Timestamp >= (ulong)startTime.Ticks && row.Timestamp < (ulong)(startTime + duration).Ticks)) { - return (long)(i * 1000000); + yield return row; } - return (long)i; } - return null; } /// diff --git a/LogViewer/LogViewer/Model/KmlDataLog.cs b/LogViewer/LogViewer/Model/KmlDataLog.cs index a31a995115..fc393d35c8 100644 --- a/LogViewer/LogViewer/Model/KmlDataLog.cs +++ b/LogViewer/LogViewer/Model/KmlDataLog.cs @@ -203,7 +203,11 @@ public IEnumerable GetDataValues(LogItemSchema schema, DateTime start { foreach (LogEntry entry in GetRows(schema.Parent.Name, startTime, duration)) { - yield return entry.GetDataValue(schema.Name); + var dv = entry.GetDataValue(schema.Name); + if (dv != null) + { + yield return dv; + } } } } diff --git a/LogViewer/LogViewer/Model/Px4DataLog.cs b/LogViewer/LogViewer/Model/Px4DataLog.cs index bb4db3da9e..43b59c348d 100644 --- a/LogViewer/LogViewer/Model/Px4DataLog.cs +++ b/LogViewer/LogViewer/Model/Px4DataLog.cs @@ -46,7 +46,16 @@ public virtual DataValue GetDataValue(string field) DataValue dv = new DataValue() { X = Timestamp, Y = 0 }; try { - dv.Y = Convert.ToDouble(result.Value); + if (result.Value is double) + { + dv.Y = (double)result.Value; + } + if (result.Value is string) + { + double y = 0; + double.TryParse((string)result.Value, out y); + dv.Y = y; + } } catch { } @@ -428,7 +437,11 @@ public IEnumerable GetDataValues(LogItemSchema schema, DateTime start { foreach (LogEntry entry in GetRows(schema.Parent.Name, startTime, duration)) { - yield return entry.GetDataValue(schema.Name); + var dv = entry.GetDataValue(schema.Name); + if (dv != null) + { + yield return dv; + } } } } diff --git a/LogViewer/LogViewer/Utilities/Generated.cs b/LogViewer/LogViewer/Utilities/Generated.cs index 4b330b945f..5271cd39ad 100644 --- a/LogViewer/LogViewer/Utilities/Generated.cs +++ b/LogViewer/LogViewer/Utilities/Generated.cs @@ -132,8 +132,18 @@ public LogEntryGPS(LogEntry entry) //double gcrs = entry.GetField("GCrs"); // e //double vz = entry.GetField("VZ"); // f //byte u = entry.GetField("U"); // B + } + + else if (entry.HasField("Time")) + { + // DJI converted .dat file format + GPSTime = entry.GetField("Time"); + nSat = entry.GetField("Visible:GPS"); + EPH = (float)entry.GetField("DOP:H"); + Lat = entry.GetField("Lat"); + Lon = entry.GetField("Long"); + Alt = (float)entry.GetField("Alt"); // e } - } } From 3e62641b18b6299ba397020cb112f3b27ab62629 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 21 Mar 2018 23:06:58 -0700 Subject: [PATCH 024/121] Car settings update from Jim Piavis --- build.cmd | 8 ++++---- setup.sh | 7 +++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/build.cmd b/build.cmd index e66a9a83fe..21d3d4533d 100644 --- a/build.cmd +++ b/build.cmd @@ -75,7 +75,7 @@ robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Rel REM //---------- get High PolyCount SUV Car Model ------------ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv mkdir Unreal\Plugins\AirSim\Content\VehicleAdv -IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.7 ( +IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.9 ( IF NOT DEFINED noFullPolyCar ( REM //leave some blank lines because powershell shows download banner at top of console ECHO( @@ -89,9 +89,9 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.7 ( IF EXIST suv_download_tmp rmdir suv_download_tmp /q /s mkdir suv_download_tmp @echo on - REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.1.7/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" - REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.1.7/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.1.7/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" + REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" + powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" @echo off rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV powershell -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }" diff --git a/setup.sh b/setup.sh index 19ea18c499..29247c601d 100755 --- a/setup.sh +++ b/setup.sh @@ -86,7 +86,7 @@ fi if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv" ]; then mkdir -p "Unreal/Plugins/AirSim/Content/VehicleAdv" fi -if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.7" ]; then +if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.9" ]; then if $downloadHighPolySuv; then echo "*********************************************************************************************" echo "Downloading high-poly car assets.... The download is ~37MB and can take some time." @@ -98,7 +98,10 @@ if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.7" ]; then fi mkdir -p "suv_download_tmp" cd suv_download_tmp - wget https://github.com/Microsoft/AirSim/releases/download/v1.1.7/car_assets.zip + wget https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip + if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then + rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" + fi unzip car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv cd .. rm -rf "suv_download_tmp" From ca54e2717a35d64264491d11f38f8a3144de1c79 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 20 Mar 2018 22:43:38 -0700 Subject: [PATCH 025/121] added constructor for CarControls --- PythonClient/AirSimClient.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 105329bbee..313f1a6a1a 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -159,6 +159,17 @@ class CarControls(MsgpackMixin): manual_gear = 0 gear_immediate = True + def __init__(self, throttle = np.float32(0), steering = np.float32(0), brake = np.float32(0), + handbrake = False, is_manual_gear = False, manual_gear = 0, gear_immediate = True): + self.throttle = throttle + self.steering = steering + self.brake = brake + self.handbrake = handbrake + self.is_manual_gear = is_manual_gear + self.manual_gear = manual_gear + self.gear_immediate = gear_immediate + + def set_throttle(self, throttle_val, forward): if (forward): is_manual_gear = False From b48422764fcfc2e424faec251e584a28c602f12e Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 22 Mar 2018 00:07:02 -0700 Subject: [PATCH 026/121] increase keyboard steering to 0.5, CarControls contructor fix --- PythonClient/AirSimClient.py | 2 +- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 313f1a6a1a..b6f689dca8 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -159,7 +159,7 @@ class CarControls(MsgpackMixin): manual_gear = 0 gear_immediate = True - def __init__(self, throttle = np.float32(0), steering = np.float32(0), brake = np.float32(0), + def __init__(self, throttle = 0, steering = 0, brake = 0, handbrake = False, is_manual_gear = False, manual_gear = 0, gear_immediate = True): self.throttle = throttle self.steering = steering diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 1cfbe5189b..f25de2b2dd 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -298,10 +298,10 @@ void ACarPawn::setupInputBindings() UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveForward", EKeys::Down, -1), this, this, &ACarPawn::MoveForward); - UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveRight", EKeys::Right, 0.1), this, + UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveRight", EKeys::Right, 0.5), this, this, &ACarPawn::MoveRight); - UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveRight", EKeys::Left, -0.1), this, + UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveRight", EKeys::Left, -0.5), this, this, &ACarPawn::MoveRight); UAirBlueprintLib::BindActionToKey("Handbrake", EKeys::End, this, &ACarPawn::OnHandbrakePressed, true); From 6ff1fd54a02de87947ba81e2708f9fa0ad362c62 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 22 Mar 2018 17:19:31 -0700 Subject: [PATCH 027/121] Add map annotation feature. --- LogViewer/LogViewer/Controls/Console.xaml.cs | 12 +- LogViewer/LogViewer/LogViewer.csproj | 2 +- LogViewer/LogViewer/MainWindow.xaml | 14 +- LogViewer/LogViewer/MainWindow.xaml.cs | 230 +++++++++++++++++-- LogViewer/LogViewer/Model/MavlinkLog.cs | 2 +- LogViewer/LogViewer/Model/Px4DataLog.cs | 13 +- LogViewer/LogViewer/Model/Px4ULog.cs | 51 +++- 7 files changed, 281 insertions(+), 43 deletions(-) diff --git a/LogViewer/LogViewer/Controls/Console.xaml.cs b/LogViewer/LogViewer/Controls/Console.xaml.cs index 1e43fc8f7b..85430b26a9 100644 --- a/LogViewer/LogViewer/Controls/Console.xaml.cs +++ b/LogViewer/LogViewer/Controls/Console.xaml.cs @@ -102,7 +102,17 @@ public void Write(string text) if ((string)run.Tag == "user") { - run = run.PreviousInline as Run; + var previousrun = run.PreviousInline as Run; + if (previousrun == null) + { + var newrun = new Run() { Foreground = ConsoleTextBrush }; + last.Inlines.InsertBefore(run, newrun); + run = newrun; + } + else + { + run = previousrun; + } } // todo: process binary console commands embedded in the text... diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 3631e16d1c..593b3f95a5 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -33,7 +33,7 @@ PX4 Log Viewer true publish.htm - 44 + 45 1.0.0.%2a false true diff --git a/LogViewer/LogViewer/MainWindow.xaml b/LogViewer/LogViewer/MainWindow.xaml index b667c32309..156a2dc27f 100644 --- a/LogViewer/LogViewer/MainWindow.xaml +++ b/LogViewer/LogViewer/MainWindow.xaml @@ -102,7 +102,7 @@ - + @@ -122,7 +122,13 @@ ItemContainerStyle="{StaticResource ContainerListItemStyle}" Style="{StaticResource NavigationList}" local:PassthroughMouseWheelBehavior.PassthroughMouseWheel="True" + PreviewMouseRightButtonDown="OnRightClickCategoryList" ScrollViewer.CanContentScroll="False"> + + + + + @@ -144,11 +150,11 @@ - + - + @@ -164,5 +170,5 @@ - + diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 2296b7b124..2918890fb1 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -39,6 +39,8 @@ public partial class MainWindow : Window MapPolyline currentFlight; MavlinkLog currentFlightLog; long lastAttitudeMessage; + List mappedLogEntries; + MapLayer annotationLayer; public MainWindow() { @@ -829,6 +831,7 @@ void ShowMap() line.StrokeThickness = 4; line.Stroke = new SolidColorBrush(GetRandomColor()); LocationCollection points = new LocationCollection(); + mappedLogEntries = new List(); //Debug.WriteLine("time,\t\tlat,\t\tlong,\t\t\tnsat,\talt,\thdop,\tfix"); foreach (var row in log.GetRows("GPS", flight.StartTime, flight.Duration)) @@ -844,6 +847,7 @@ void ShowMap() alt = 0; } mapData.Add(gps); + mappedLogEntries.Add(row); var pos = new Location() { Altitude = alt, Latitude = gps.Lat, Longitude = gps.Lon }; points.Add(pos); ulong time = (ulong)gps.GPSTime; @@ -953,8 +957,6 @@ private void OnChildListItemSelected(object sender, SelectionChangedEventArgs e) IEnumerable GetSelectedDataValues(LogItemSchema schema) { - List combined = new List(); - List selected = GetSelectedFlights(); if (selected.Count == 0) { @@ -970,13 +972,14 @@ IEnumerable GetSelectedDataValues(LogItemSchema schema) { if (flight.Log == null || flight.Log == log) { - combined.AddRange(log.GetDataValues(schema, flight.StartTime, flight.Duration)); + foreach (var dv in log.GetDataValues(schema, flight.StartTime, flight.Duration)) + { + yield return dv; + } } } } - } - - return combined; + } } Thickness defaultChartMargin = new Thickness(0, 10, 0, 10); @@ -1100,22 +1103,184 @@ private void GraphItem(LogItemSchema schema) else { StringBuilder sb = new StringBuilder(); + string previous = null; + List unique = new List(); foreach (var value in GetSelectedDataValues(schema)) { if (!string.IsNullOrEmpty(value.Label)) { - sb.AppendLine(value.Label); + if (previous != value.Label) + { + unique.Add(value); + sb.Append(((ulong)value.X).ToString()); + sb.Append(": "); + sb.AppendLine(value.Label); + previous = value.Label; + } + } + } + SystemConsole.Write(sb.ToString()); + ConsoleButton.IsChecked = true; + SystemConsole.Show(); + } + } + + private void AnnotateMap(LogItemSchema schema) + { + List unique = new List(); + if (schema.IsNumeric) + { + var data = GetSelectedDataValues(schema); + ShowStatus(string.Format("Found {0} data values", data.Count())); + if (data.Count() > 0) + { + double previous = 0; + { + // uniquify it. + foreach (var value in data) + { + if (value.Y != previous) + { + unique.Add(value); + previous = value.Y; + } + } + } + } + } + else + { + StringBuilder sb = new StringBuilder(); + string previous = null; + foreach (var value in GetSelectedDataValues(schema)) + { + if (!string.IsNullOrEmpty(value.Label)) + { + if (previous != value.Label) + { + unique.Add(value); + sb.Append(value.X.ToString()); + sb.Append(": "); + sb.AppendLine(value.Label); + previous = value.Label; + } + } + } + } + + // if there are too many values, then limit it to an even spread of 100 items. + if (unique.Count > 100) + { + var summary = new List(); + double skip = (unique.Count / 100); + for (int i = 0, n = unique.Count; i < n; i++) + { + var value = unique[i]; + if (i >= summary.Count * unique.Count / 100) + { + summary.Add(value); + } + } + unique = summary; + } + AnnotateMap(unique); + } + + private void AnnotateMap(List unique) + { + if (this.mappedLogEntries == null || this.mappedLogEntries.Count == 0) + { + ShowMap(); + } + + if (this.mappedLogEntries == null || this.mappedLogEntries.Count == 0) + { + MessageBox.Show("Sorry, could not find GPS map info, so cannot annotate data on the map", + "GPS info is missing", MessageBoxButton.OK, MessageBoxImage.Exclamation); + return; + } + + if (annotationLayer != null) + { + myMap.Children.Remove(annotationLayer); + } + annotationLayer = new MapLayer(); + + SolidColorBrush annotationBrush = new SolidColorBrush(Color.FromArgb(0x80, 0xff, 0xff, 0xB0)); + + foreach (var dv in unique) + { + LogEntry closest = null; + LogField field = dv.UserData as LogField; + if (field != null) + { + // csv log + LogEntry e = field.Parent; + closest = FindNearestMappedItem(e.Timestamp); + } + else + { + // px4 log? + Message msg = dv.UserData as Message; + if (msg != null) + { + closest = FindNearestMappedItem(msg.GetTimestamp()); } else { - sb.AppendLine(value.Y.ToString()); + // mavlink + MavlinkLog.Message mavmsg = dv.UserData as MavlinkLog.Message; + if (mavmsg != null) + { + closest = FindNearestMappedItem(mavmsg.Timestamp.Ticks / 10); + } } } - SystemConsole.Write(sb.ToString()); - ConsoleButton.IsChecked = true; - SystemConsole.Show(); + if (closest != null) + { + LogEntryGPS gps = new LogEntryGPS(closest); + // map doesn't like negative altitudes. + double alt = gps.Alt; + if (alt < 0) + { + alt = 0; + } + var pos = new Location() { Altitude = alt, Latitude = gps.Lat, Longitude = gps.Lon }; + string label = dv.Label; + if (string.IsNullOrEmpty(label)) + { + label = dv.Y.ToString(); + } + annotationLayer.AddChild(new TextBlock(new Run(label) { Background = annotationBrush }), pos, PositionOrigin.BottomLeft); + } + } + myMap.Children.Add(annotationLayer); + + SystemConsole.Hide(); + ChartStack.Visibility = Visibility.Collapsed; + myMap.Visibility = Visibility.Visible; + myMap.UpdateLayout(); + } + + private LogEntry FindNearestMappedItem(double t) + { + LogEntry closest = null; + double bestDiff = 0; + // find nearest mapped location (nearest in time). + foreach (var mapped in this.mappedLogEntries) + { + var time = mapped.Timestamp; + var diff = Math.Abs((double)time - t); + + if (closest == null || diff < bestDiff) + { + closest = mapped; + bestDiff = diff; + } + } + return closest; } private void OnNewChartGenerated(object sender, List e) @@ -1178,6 +1343,10 @@ private void LayoutCharts() double height = ChartStack.ActualHeight; double count = ChartStack.ChartCount; height -= (count * (defaultChartMargin.Top + defaultChartMargin.Bottom)); // remove margins + if (height < 0) + { + height = 0; + } double chartHeight = Math.Min(MaxChartHeight, height / count); bool found = false; foreach (FrameworkElement c in ChartStack.Charts) @@ -1250,6 +1419,7 @@ private void UnselectCategory(LogItemSchema item) } } + private void OnClear(object sender, RoutedEventArgs e) { ChartStack.ClearCharts(); @@ -1294,15 +1464,7 @@ private void OnFlightSelected(object sender, SelectionChangedEventArgs e) } }); } - - //private void OnMapPointerMoved(object sender, PointerRoutedEventArgs e) - //{ - // Point mapPos = e.GetCurrentPoint(myMap).Position; - // Geopoint location; - // myMap.GetLocationFromOffset(mapPos, out location); - // StatusText.Text = location.Position.Latitude + ", " + location.Position.Longitude; - //} - + private void OnFlightViewKeyDown(object sender, KeyEventArgs e) { if (e.Key == Key.Delete) @@ -1487,6 +1649,8 @@ struct IncomingImage IncomingImage incoming_image = new IncomingImage(); + public LogItemSchema rightClickedItem { get; private set; } + private void OnShowConsole(object sender, RoutedEventArgs e) { SystemConsole.Show(); @@ -1583,7 +1747,6 @@ private void DrawVectors(double[,] xmag, double[,] ymag) // find guassian lines in the map and draw them so it looks like this: // https://www.ngdc.noaa.gov/geomag/WMM/data/WMM2015/WMM2015_D_MERC.pdf - for (int i = 0; i < 180; i++) { for (int j = 0; j < 360; j++) @@ -1619,6 +1782,31 @@ private void OnPaste(object sender, ExecutedRoutedEventArgs e) CameraPanel.Visibility = Visibility.Visible; } } + + private void OnAnnotateItem(object sender, RoutedEventArgs e) + { + LogItemSchema item = this.rightClickedItem; + if (item != null) + { + AnnotateMap(item); + } + } + + private void OnRightClickCategoryList(object sender, MouseButtonEventArgs e) + { + this.rightClickedItem = null; + Point pos = e.GetPosition(CategoryList); + DependencyObject dep = (DependencyObject)e.OriginalSource; + while ((dep != null) && !(dep is ListViewItem)) + { + dep = VisualTreeHelper.GetParent(dep); + } + if (dep == null) + return; + ListViewItem listitem = (ListViewItem)dep; + LogItemSchema item = listitem.DataContext as LogItemSchema; + this.rightClickedItem = item; + } } } diff --git a/LogViewer/LogViewer/Model/MavlinkLog.cs b/LogViewer/LogViewer/Model/MavlinkLog.cs index 454e8cfa80..b70a6b9788 100644 --- a/LogViewer/LogViewer/Model/MavlinkLog.cs +++ b/LogViewer/LogViewer/Model/MavlinkLog.cs @@ -94,7 +94,7 @@ internal DataValue GetDataValue(LogItemSchema schema, Message msg) if (fi != null) { object value = fi.GetValue(row); - DataValue data = new DataValue() { X = msg.Timestamp.Ticks / 10, UserData = row }; // microseconds (Ticks are in 100 nanoseconds). + DataValue data = new DataValue() { X = msg.Timestamp.Ticks / 10, UserData = msg }; // microseconds (Ticks are in 100 nanoseconds). // byte array is special (we treat this like text). if (value is byte[]) diff --git a/LogViewer/LogViewer/Model/Px4DataLog.cs b/LogViewer/LogViewer/Model/Px4DataLog.cs index 43b59c348d..2c67b40d98 100644 --- a/LogViewer/LogViewer/Model/Px4DataLog.cs +++ b/LogViewer/LogViewer/Model/Px4DataLog.cs @@ -17,6 +17,7 @@ public class LogField { public string Name { get; set; } public object Value { get; set; } + public LogEntry Parent { get; internal set; } } public class LogEntry : LogField @@ -43,7 +44,7 @@ public virtual DataValue GetDataValue(string field) } if (result != null) { - DataValue dv = new DataValue() { X = Timestamp, Y = 0 }; + DataValue dv = new DataValue() { X = Timestamp, Y = 0, UserData = result }; try { if (result.Value is double) @@ -292,12 +293,13 @@ internal void SetField(string name, object value) { if (cache == null) { - cache = new Dictionary(); + cache = new Dictionary(); } - cache[name] = new Model.LogField() + cache[name] = new LogField() { Name = name, - Value = value + Value = value, + Parent = this }; } @@ -401,7 +403,8 @@ private void ParseFields() cache[Format.Columns[k]] = new LogField() { Name = Format.Columns[k], - Value = value + Value = value, + Parent = this }; } } diff --git a/LogViewer/LogViewer/Model/Px4ULog.cs b/LogViewer/LogViewer/Model/Px4ULog.cs index 6b790194e0..849065e5f3 100644 --- a/LogViewer/LogViewer/Model/Px4ULog.cs +++ b/LogViewer/LogViewer/Model/Px4ULog.cs @@ -10,9 +10,9 @@ namespace LogViewer.Model.ULog { - public class Message + public abstract class Message { - + public abstract double GetTimestamp(); } public enum FieldType @@ -174,6 +174,11 @@ public class MessageFormat : Message public int id; public List fields = new List(); + public override double GetTimestamp() + { + return 0; + } + public MessageFormat(string fmt) { this.fmt = fmt; @@ -218,6 +223,12 @@ public MessageLogging(byte logLevel, long timestamp, string msg) this.msg = msg; } + public override double GetTimestamp() + { + return timestamp; + } + + } class MessageData : Message @@ -236,6 +247,16 @@ public MessageData(UInt16 msgId, byte[] value, MessageSubscription s) this.format = s.format; } + public override double GetTimestamp() + { + object ts = null; + if (values.TryGetValue("timestamp", out ts)) + { + return Convert.ToDouble(ts); + } + return 0; + } + internal DataValue GetValue(MessageField field) { if (values == null) @@ -243,13 +264,8 @@ internal DataValue GetValue(MessageField field) ParseValues(); } - double x = 0; + double x = GetTimestamp(); double y = 0; - object ts = null; - if (values.TryGetValue("timestamp", out ts)) - { - x = Convert.ToDouble(ts); - } object o = null; if (values.TryGetValue(field.name, out o)) @@ -507,8 +523,11 @@ public MessageInfo(string key, byte[] value) this.key = key; this.value = value; } - - + public override double GetTimestamp() + { + return 0; + } + } class MessageParameter : Message @@ -523,6 +542,10 @@ public MessageParameter(string key, byte[] value) } + public override double GetTimestamp() + { + return 0; + } } class MessageSync : Message @@ -535,6 +558,10 @@ public MessageSync(byte[] magic) } + public override double GetTimestamp() + { + return 0; + } } class MessageDropOut : Message @@ -547,6 +574,10 @@ public MessageDropOut(UInt16 duration) } + public override double GetTimestamp() + { + return 0; + } } class MessageSubscription From 012c75360601849cd2503d4a870cc6489c048f7e Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 22 Mar 2018 20:53:05 -0700 Subject: [PATCH 028/121] rename moveByAngle -> moveByAngleZ --- .../vehicles/multirotor/api/MultirotorApi.hpp | 12 +++++----- .../multirotor/api/MultirotorRpcLibClient.hpp | 2 +- .../controllers/DroneControllerBase.hpp | 4 ++-- .../multirotor/api/MultirotorRpcLibClient.cpp | 4 ++-- .../multirotor/api/MultirotorRpcLibServer.cpp | 4 ++-- .../controllers/DroneControllerBase.cpp | 2 +- DroneShell/src/main.cpp | 22 +++++++++---------- PythonClient/AirSimClient.py | 4 ++-- 8 files changed, 27 insertions(+), 27 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index d24eeb2d93..1de732384f 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -18,7 +18,7 @@ using namespace msr::airlib; namespace msr { namespace airlib { -// We want to make it possible for MultirotorRpcLibClient to call the offboard movement methods (moveByAngle, moveByVelocity, etc) at a high +// We want to make it possible for MultirotorRpcLibClient to call the offboard movement methods (moveByAngleZ, moveByVelocity, etc) at a high // rate, like 30 times a second. But we also want these movement methods to drive the drone at a reliable rate which we do inside // DroneControllerBase using the Waiter object so it pumps the virtual commandVelocity method at a fixed rate defined by getCommandPeriod. // This fixed rate is needed by the drone flight controller (for example PX4) because the flight controller usually reverts to @@ -83,9 +83,9 @@ class MultirotorApi : public VehicleApiBase { return controller_->goHome(*pending_); } - bool moveByAngle(float pitch, float roll, float z, float yaw, float duration) + bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) { - std::shared_ptr cmd = std::make_shared(controller_, pitch, roll, z, yaw, duration); + std::shared_ptr cmd = std::make_shared(controller_, pitch, roll, z, yaw, duration); return enqueueCommand(cmd); } @@ -384,10 +384,10 @@ class MultirotorApi : public VehicleApiBase { return true; } - class MoveByAngle : public OffboardCommand { + class MoveByAngleZ : public OffboardCommand { float pitch_, roll_, z_, yaw_, duration_; public: - MoveByAngle(DroneControllerBase* controller, float pitch, float roll, float z, float yaw, float duration) : OffboardCommand(controller) { + MoveByAngleZ(DroneControllerBase* controller, float pitch, float roll, float z, float yaw, float duration) : OffboardCommand(controller) { this->pitch_ = pitch; this->roll_ = roll; this->z_ = z; @@ -395,7 +395,7 @@ class MultirotorApi : public VehicleApiBase { this->duration_ = duration; } virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override { - controller->moveByAngle(pitch_, roll_, z_, yaw_, duration_, cancelable); + controller->moveByAngleZ(pitch_, roll_, z_, yaw_, duration_, cancelable); } }; diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index fe2d6a9b1f..e09a577ed9 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -23,7 +23,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase { bool takeoff(float max_wait_ms = 15); bool land(float max_wait_seconds = 60); bool goHome(); - bool moveByAngle(float pitch, float roll, float z, float yaw, float duration); + bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode()); diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 0747f97491..445c36700a 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -102,10 +102,10 @@ class DroneControllerBase : public VehicleControllerBase { /// make the drone move forwards, a little bit of roll can make it move sideways. The yaw control can /// make the drone spin around on the spot. The duration says how long you want to apply these settings /// before reverting to a hover command. So you can say "fly forwards slowly for 1 second" using - /// moveByAngle(0.1, 0, z, yaw, 1, ...). The cancelable_action can be used to canel all actions. In fact, + /// moveByAngleZ(0.1, 0, z, yaw, 1, ...). The cancelable_action can be used to canel all actions. In fact, /// every time you call another move* method you will automatically cancel any previous action that is /// happening. - virtual bool moveByAngle(float pitch, float roll, float z, float yaw, float duration + virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration , CancelableBase& cancelable_action); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 41ddf38faa..529130cd72 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -61,9 +61,9 @@ bool MultirotorRpcLibClient::goHome() return static_cast(getClient())->call("goHome").as(); } -bool MultirotorRpcLibClient::moveByAngle(float pitch, float roll, float z, float yaw, float duration) +bool MultirotorRpcLibClient::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration) { - return static_cast(getClient())->call("moveByAngle", pitch, roll, z, yaw, duration).as(); + return static_cast(getClient())->call("moveByAngleZ", pitch, roll, z, yaw, duration).as(); } bool MultirotorRpcLibClient::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 7c2afc305b..2c7829c1f8 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -46,8 +46,8 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv (static_cast(getServer()))-> - bind("moveByAngle", [&](float pitch, float roll, float z, float yaw, float duration) -> - bool { return getDroneApi()->moveByAngle(pitch, roll, z, yaw, duration); }); + bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration) -> + bool { return getDroneApi()->moveByAngleZ(pitch, roll, z, yaw, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) -> bool { return getDroneApi()->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); }); diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index c10595dd15..07c06b9f13 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -46,7 +46,7 @@ void DroneControllerBase::loopCommandPost() //no-op by default. derived class can override it if needed } -bool DroneControllerBase::moveByAngle(float pitch, float roll, float z, float yaw, float duration +bool DroneControllerBase::moveByAngleZ(float pitch, float roll, float z, float yaw, float duration , CancelableBase& cancelable_action) { if (duration <= 0) diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 5e2cc27452..12b28b2c17 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -482,9 +482,9 @@ class MoveByManualCommand : public DroneCommand { }; -class MoveByAngleCommand : public DroneCommand { +class MoveByAngleZCommand : public DroneCommand { public: - MoveByAngleCommand() : DroneCommand("MoveByAngle", "Move with specified roll and pitch, leaving z as-is") + MoveByAngleZCommand() : DroneCommand("MoveByAngleZ", "Move with specified roll and pitch, leaving z as-is") { this->addSwitch({"-pitch", "0", "pitch angle in degrees (default 0)" }); this->addSwitch({"-roll", "0", "roll angle in degrees (default 0)" }); @@ -503,7 +503,7 @@ class MoveByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - context->client.moveByAngle(pitch, roll, z, yaw, duration); + context->client.moveByAngleZ(pitch, roll, z, yaw, duration); }); return false; @@ -662,12 +662,12 @@ class BackForthByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - if (!context->client.moveByAngle(pitch, roll, z, yaw, duration)) { + if (!context->client.moveByAngleZ(pitch, roll, z, yaw, duration)) { throw std::runtime_error("BackForthByAngleCommand cancelled"); } context->client.hover(); context->sleep_for(pause_time); - if (!context->client.moveByAngle(-pitch, -roll, z, yaw, duration)){ + if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, duration)){ throw std::runtime_error("BackForthByAngleCommand cancelled"); } }, iterations); @@ -747,22 +747,22 @@ class SquareByAngleCommand : public DroneCommand { CommandContext* context = params.context; context->tasker.execute([=]() { - if (!context->client.moveByAngle(pitch, -roll, z, yaw, 0)) { + if (!context->client.moveByAngleZ(pitch, -roll, z, yaw, 0)) { throw std::runtime_error("SquareByAngleCommand cancelled"); } context->client.hover(); context->sleep_for(pause_time); - if (!context->client.moveByAngle(-pitch, -roll, z, yaw, 0)) { + if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)) { throw std::runtime_error("SquareByAngleCommand cancelled"); } context->client.hover(); context->sleep_for(pause_time); - if (!context->client.moveByAngle(-pitch, roll, z, yaw, 0)) { + if (!context->client.moveByAngleZ(-pitch, roll, z, yaw, 0)) { throw std::runtime_error("SquareByAngleCommand cancelled"); } context->client.hover(); context->sleep_for(pause_time); - if (!context->client.moveByAngle(-pitch, -roll, z, yaw, 0)){ + if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)){ throw std::runtime_error("SquareByAngleCommand cancelled"); } context->client.hover(); @@ -1351,7 +1351,7 @@ int main(int argc, const char *argv[]) { MoveToPositionCommand moveToPosition; GetPositionCommand getPosition; MoveByManualCommand moveByManual; - MoveByAngleCommand moveByAngle; + MoveByAngleZCommand moveByAngleZ; MoveByVelocityCommand moveByVelocity; MoveByVelocityZCommand moveByVelocityZ; MoveOnPathCommand moveOnPath; @@ -1383,7 +1383,7 @@ int main(int argc, const char *argv[]) { shell.addCommand(hover); shell.addCommand(moveToPosition); shell.addCommand(moveByManual); - shell.addCommand(moveByAngle); + shell.addCommand(moveByAngleZ); shell.addCommand(moveByVelocity); shell.addCommand(moveByVelocityZ); shell.addCommand(moveOnPath); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index b6f689dca8..787571e054 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -547,8 +547,8 @@ def getServerDebugInfo(self): # APIs for control - def moveByAngle(self, pitch, roll, z, yaw, duration): - return self.client.call('moveByAngle', pitch, roll, z, yaw, duration) + def moveByAngleZ(self, pitch, roll, z, yaw, duration): + return self.client.call('moveByAngleZ', pitch, roll, z, yaw, duration) def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): return self.client.call('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode) From b4a934b9dd49f7e0edddd364a3080bc5e243569f Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 22 Mar 2018 21:43:19 -0700 Subject: [PATCH 029/121] added moveByAngleThrottle command --- .../vehicles/multirotor/api/MultirotorApi.hpp | 21 +++++++++++++ .../multirotor/api/MultirotorRpcLibClient.hpp | 1 + .../controllers/DroneControllerBase.hpp | 5 +++ .../controllers/MavLinkDroneController.hpp | 10 ++++++ .../ros_flight/RosFlightDroneController.hpp | 10 ++++++ .../SimpleFlightDroneController.hpp | 13 ++++++++ .../multirotor/api/MultirotorRpcLibClient.cpp | 5 +++ .../multirotor/api/MultirotorRpcLibServer.cpp | 3 ++ .../controllers/DroneControllerBase.cpp | 19 ++++++++++++ DroneShell/src/main.cpp | 31 +++++++++++++++++++ PythonClient/AirSimClient.py | 3 ++ 11 files changed, 121 insertions(+) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index 1de732384f..aa688eac56 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -89,6 +89,12 @@ class MultirotorApi : public VehicleApiBase { return enqueueCommand(cmd); } + bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) + { + std::shared_ptr cmd = std::make_shared(controller_, pitch, roll, throttle, yaw_rate, duration); + return enqueueCommand(cmd); + } + bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { std::shared_ptr cmd = std::make_shared(controller_, vx, vy, vz, duration, drivetrain, yaw_mode); @@ -384,6 +390,21 @@ class MultirotorApi : public VehicleApiBase { return true; } + class MoveByAngleThrottle : public OffboardCommand { + float pitch_, roll_, throttle_, yaw_rate_, duration_; + public: + MoveByAngleThrottle(DroneControllerBase* controller, float pitch, float roll, float throttle, float yaw_rate, float duration) : OffboardCommand(controller) { + this->pitch_ = pitch; + this->roll_ = roll; + this->throttle_ = throttle; + this->yaw_rate_ = yaw_rate; + this->duration_ = duration; + } + virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override { + controller->moveByAngleThrottle (pitch_, roll_, throttle_, yaw_rate_, duration_, cancelable); + } + }; + class MoveByAngleZ : public OffboardCommand { float pitch_, roll_, z_, yaw_, duration_; public: diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index e09a577ed9..ed07d69a61 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -24,6 +24,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase { bool land(float max_wait_seconds = 60); bool goHome(); bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration); + bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration); bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode()); diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 445c36700a..77d94644a7 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -108,6 +108,9 @@ class DroneControllerBase : public VehicleControllerBase { virtual bool moveByAngleZ(float pitch, float roll, float z, float yaw, float duration , CancelableBase& cancelable_action); + /// Move by providing angles and throttles just lik ein RC + virtual bool moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration + , CancelableBase& cancelable_action); /// Move the drone by controlling the velocity vector of the drone. A little bit of vx can /// make the drone move forwards, a little bit of vy can make it move sideways. A bit of vz can move @@ -240,6 +243,7 @@ class DroneControllerBase : public VehicleControllerBase { //all angles in degrees, lengths in meters, velocities in m/s, durations in seconds //all coordinates systems are world NED (+x is North, +y is East, +z is down) virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0; + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) = 0; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; @@ -272,6 +276,7 @@ class DroneControllerBase : public VehicleControllerBase { virtual bool moveByVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode); virtual bool moveToPosition(const Vector3r& dest, const YawMode& yaw_mode); virtual bool moveByRollPitchZ(float pitch, float roll, float z, float yaw); + virtual bool moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate); //**************************************************************************************************************************** /************* safety checks & emergency manuevers ************/ diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp index 8cd4b3968d..91a4c7fb44 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp @@ -144,6 +144,7 @@ class MavLinkDroneController : public DroneControllerBase virtual void loopCommandPost() override; protected: virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override; + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override; virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override; @@ -1165,6 +1166,11 @@ struct MavLinkDroneController::impl { return rc; } + void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) + { + checkVehicle(); + mav_vehicle_->moveByAttitude(roll, pitch, yaw_rate, 0, 0, 0, throttle); + } void commandRollPitchZ(float pitch, float roll, float z, float yaw) { if (target_height_ != -z) { @@ -1490,6 +1496,10 @@ bool MavLinkDroneController::goHome(CancelableBase& cancelable_action) return pimpl_->goHome(cancelable_action); } +void MavLinkDroneController::commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +{ + return pimpl_->commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); +} void MavLinkDroneController::commandRollPitchZ(float pitch, float roll, float z, float yaw) { return pimpl_->commandRollPitchZ(pitch, roll, z, yaw); diff --git a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp index f7eeef8024..5c5a6fda58 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp @@ -249,6 +249,16 @@ class RosFlightDroneController : public DroneControllerBase { } protected: + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + { + unused(pitch); + unused(roll); + unused(throttle); + unused(yaw_rate); + + //TODO: implement this + } + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override { unused(pitch); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 08b824d429..b98c7a91cf 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -225,6 +225,19 @@ class SimpleFlightDroneController : public DroneControllerBase { } protected: + virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override + { + Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + + typedef simple_flight::GoalModeType GoalModeType; + simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); + + simple_flight::Axis4r goal(roll, pitch, yaw_rate, throttle); + + std::string message; + firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); + } + virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override { Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 529130cd72..46df2b0c89 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -66,6 +66,11 @@ bool MultirotorRpcLibClient::moveByAngleZ(float pitch, float roll, float z, floa return static_cast(getClient())->call("moveByAngleZ", pitch, roll, z, yaw, duration).as(); } +bool MultirotorRpcLibClient::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration) +{ + return static_cast(getClient())->call("moveByAngleThrottle", pitch, roll, throttle, yaw_rate, duration).as(); +} + bool MultirotorRpcLibClient::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { return static_cast(getClient())->call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode)).as(); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 2c7829c1f8..bee4df9ba2 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -48,6 +48,9 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv (static_cast(getServer()))-> bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration) -> bool { return getDroneApi()->moveByAngleZ(pitch, roll, z, yaw, duration); }); + (static_cast(getServer()))-> + bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration) -> + bool { return getDroneApi()->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); }); (static_cast(getServer()))-> bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) -> bool { return getDroneApi()->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); }); diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index 07c06b9f13..73f0e75733 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -57,6 +57,17 @@ bool DroneControllerBase::moveByAngleZ(float pitch, float roll, float z, float y }, duration, cancelable_action); } +bool DroneControllerBase::moveByAngleThrottle(float pitch, float roll, float throttle, float yaw_rate, float duration + , CancelableBase& cancelable_action) +{ + if (duration <= 0) + return true; + + return !waitForFunction([&]() { + return !moveByRollPitchThrottle(pitch, roll, throttle, yaw_rate); + }, duration, cancelable_action); +} + bool DroneControllerBase::moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action) { @@ -374,6 +385,14 @@ bool DroneControllerBase::moveToPosition(const Vector3r& dest, const YawMode& ya return true; } +bool DroneControllerBase::moveByRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) +{ + if (safetyCheckVelocity(getVelocity())) + commandRollPitchThrottle(pitch, roll, throttle, yaw_rate); + + return true; +} + bool DroneControllerBase::moveByRollPitchZ(float pitch, float roll, float z, float yaw) { if (safetyCheckVelocity(getVelocity())) diff --git a/DroneShell/src/main.cpp b/DroneShell/src/main.cpp index 12b28b2c17..a51e6ea775 100644 --- a/DroneShell/src/main.cpp +++ b/DroneShell/src/main.cpp @@ -510,6 +510,35 @@ class MoveByAngleZCommand : public DroneCommand { } }; + +class MoveByAngleThrottleCommand : public DroneCommand { +public: + MoveByAngleThrottleCommand() : DroneCommand("MoveByAngleThrottle", "Move with specified roll and pitch, leaving z as-is") + { + this->addSwitch({ "-pitch", "0", "pitch angle in degrees (default 0)" }); + this->addSwitch({ "-roll", "0", "roll angle in degrees (default 0)" }); + this->addSwitch({ "-yaw_rate", "0", "target yaw rate in degrees/sec (default is 0)" }); + this->addSwitch({ "-throttle", "-2.5", "z position in meters (default -2.5)" }); + this->addSwitch({ "-duration", "5", "the duration of this command in seconds (default 5)" }); + } + + bool execute(const DroneCommandParameters& params) + { + float pitch = getSwitch("-pitch").toFloat(); + float roll = getSwitch("-roll").toFloat(); + float yaw_rate = getSwitch("-yaw_rate").toFloat(); + float throttle = getSwitch("-throttle").toFloat(); + float duration = getSwitch("-duration").toFloat(); + CommandContext* context = params.context; + + context->tasker.execute([=]() { + context->client.moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); + }); + + return false; + } +}; + class MoveByVelocityCommand : public DroneCommand { public: MoveByVelocityCommand() : DroneCommand("MoveByVelocity", "Move by specified velocity components vx, vy, vz, axis wrt body") @@ -1352,6 +1381,7 @@ int main(int argc, const char *argv[]) { GetPositionCommand getPosition; MoveByManualCommand moveByManual; MoveByAngleZCommand moveByAngleZ; + MoveByAngleThrottleCommand moveByAngleThrottle; MoveByVelocityCommand moveByVelocity; MoveByVelocityZCommand moveByVelocityZ; MoveOnPathCommand moveOnPath; @@ -1384,6 +1414,7 @@ int main(int argc, const char *argv[]) { shell.addCommand(moveToPosition); shell.addCommand(moveByManual); shell.addCommand(moveByAngleZ); + shell.addCommand(moveByAngleThrottle); shell.addCommand(moveByVelocity); shell.addCommand(moveByVelocityZ); shell.addCommand(moveOnPath); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 787571e054..0072811df0 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -550,6 +550,9 @@ def getServerDebugInfo(self): def moveByAngleZ(self, pitch, roll, z, yaw, duration): return self.client.call('moveByAngleZ', pitch, roll, z, yaw, duration) + def moveByAngleThrottle(self, pitch, roll, throttle, yaw_rate, duration): + return self.client.call('moveByAngleThrottle', pitch, roll, throttle, yaw_rate, duration) + def moveByVelocity(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode()): return self.client.call('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode) From 9b134618b05cff2b4b382c044fa9888638334edb Mon Sep 17 00:00:00 2001 From: Kaz Walker Date: Sun, 25 Mar 2018 11:07:08 -0700 Subject: [PATCH 030/121] Fix compile errors on Xcode 8.3.x If compiling with Xcode 8.3.x (I used 8.3.2) to use Metal Shaders, an error is generated that suggests the following fix. After making this change, I was able to successfully package a game. --- .../simple_flight/firmware/AdaptiveController.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 26b546a813..1f3b8baa01 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -117,13 +117,13 @@ class AdaptiveController : public IController { // bias modification for level imu implementing deadband - if (abs(phi_in) <= 0.0001) + if (std::abs(phi_in) <= 0.0001) phi_in = 0; - if (abs(theta_in) <= 0.00001) + if (std::abs(theta_in) <= 0.00001) theta_in = 0; - if (abs(psi_in) <= 0.0001) + if (std::abs(psi_in) <= 0.0001) psi_in = 0; } @@ -581,7 +581,7 @@ class AdaptiveController : public IController { // Rescale such that the outputs normalize from -1,1 - U1 = sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal + U1 = sqrt(std::abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal U2 = U2 / 80; @@ -678,4 +678,4 @@ class AdaptiveController : public IController { } -#endif \ No newline at end of file +#endif From fc5d8d8dea353765fd423e86df645982ed7fd64d Mon Sep 17 00:00:00 2001 From: Martin Escarra Date: Mon, 26 Mar 2018 19:59:06 +0000 Subject: [PATCH 031/121] Replacing fabsf in standard lib for the one in libmath --- MavLinkCom/MavLinkTest/Commands.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MavLinkCom/MavLinkTest/Commands.cpp b/MavLinkCom/MavLinkTest/Commands.cpp index 30b3f234f3..5b4cfb908b 100644 --- a/MavLinkCom/MavLinkTest/Commands.cpp +++ b/MavLinkCom/MavLinkTest/Commands.cpp @@ -1607,11 +1607,11 @@ void GotoCommand::HandleMessage(const MavLinkMessage& message) channel->moveToLocalPosition(tx, ty, tz, is_yaw, static_cast(theading * M_PI / 180)); if (this->hasLocalPosition) { - if (!targetReached && std::fabsf(x - tx) < nearDelta && std::fabsf(y - ty) < nearDelta) + if (!targetReached && fabsf(x - tx) < nearDelta && fabsf(y - ty) < nearDelta) { targetReached = true; } - if (targetReached && !settled && (fabs(this->vx) + std::fabsf(this->vy) + std::fabsf(this->vz) < almostStationery)) { + if (targetReached && !settled && (fabs(this->vx) + fabsf(this->vy) + fabsf(this->vz) < almostStationery)) { settled = true; // ok, now we can safely switch to loiter. TargetReached(); @@ -2081,7 +2081,7 @@ void SquareCommand::UpdateTarget() float dy = ty - y; float dist = sqrtf((dx*dx) + (dy*dy)); - if (std::fabsf(dx) < near && std::fabsf(dy) < near) + if (fabsf(dx) < near && fabsf(dy) < near) { leg_++; if (leg_ == 4) leg_ = 0; From 9566732c6d56d4ce6137b5a8dd18da583cd3231e Mon Sep 17 00:00:00 2001 From: Martin Escarra Date: Mon, 26 Mar 2018 20:00:34 +0000 Subject: [PATCH 032/121] Removing explicit link against llvm's c++abi lib (this depends on the compiler chosen) --- cmake/MavLinkCom/MavLinkTest/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/MavLinkCom/MavLinkTest/CMakeLists.txt b/cmake/MavLinkCom/MavLinkTest/CMakeLists.txt index 35d3ed2f2a..e361ecb8ef 100644 --- a/cmake/MavLinkCom/MavLinkTest/CMakeLists.txt +++ b/cmake/MavLinkCom/MavLinkTest/CMakeLists.txt @@ -17,4 +17,4 @@ include_directories( file(GLOB_RECURSE PROJECT_CPP "${AIRSIM_ROOT}/MavLinkCom/${PROJECT_NAME}/*.cpp") add_executable(${PROJECT_NAME} ${PROJECT_CPP}) -target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} MavLinkCom c++abi ${CXX_EXP_LIB}) +target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} MavLinkCom ${CXX_EXP_LIB}) From 6df6be876595cb4c6250433745fbf48e0c35d337 Mon Sep 17 00:00:00 2001 From: Martin Escarra Date: Mon, 26 Mar 2018 20:01:07 +0000 Subject: [PATCH 033/121] ferror-limit is a clang specific flag, moving it inside clang conditional --- cmake/cmake-modules/CommonSetup.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index b0f144f689..71d43cb384 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -64,7 +64,7 @@ macro(CommonSetup) -std=c++14 -ggdb -Wall -Wextra -Wstrict-aliasing -Wunreachable-code -Wcast-qual -Wctor-dtor-privacy \ -Wdisabled-optimization -Wformat=2 -Winit-self -Wmissing-include-dirs -Wswitch-default \ -Wold-style-cast -Woverloaded-virtual -Wredundant-decls -Wshadow -Wstrict-overflow=5 -Wswitch-default -Wundef \ - -Wno-variadic-macros -Wno-parentheses -Wno-unused-function -Wno-unused -Wno-documentation -fdiagnostics-show-option -ferror-limit=10 \ + -Wno-variadic-macros -Wno-parentheses -Wno-unused-function -Wno-unused -Wno-documentation -fdiagnostics-show-option \ -pthread \ ${RPC_LIB_DEFINES} ${CMAKE_CXX_FLAGS}") @@ -72,7 +72,7 @@ macro(CommonSetup) # make sure to match the compiler flags with which the Unreal # Engine is built with set(CMAKE_CXX_FLAGS "\ - -nostdinc++ -isystem ${LIBCXX_INC_PATH} \ + -nostdinc++ -ferror-limit=10 -isystem ${LIBCXX_INC_PATH} \ -D__CLANG__ ${CMAKE_CXX_FLAGS}") # removed -lsupc++ from below (Git issue # 678) From 5a32bb1c413dcfc99e8e593ad80d63a807e47b85 Mon Sep 17 00:00:00 2001 From: Martin Escarra Date: Mon, 26 Mar 2018 20:03:51 +0000 Subject: [PATCH 034/121] Fixing the trailing whitespace cmake error --- cmake/cmake-modules/CommonSetup.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index 71d43cb384..5be848353d 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -89,7 +89,7 @@ macro(CommonSetup) endif() else() - set(CXX_EXP_LIB "-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel ") + set(CXX_EXP_LIB "-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel") endif () endif () From bf7dfad4c7b9d3eb1c790815b886d1e79428d23b Mon Sep 17 00:00:00 2001 From: Martin Escarra Date: Mon, 26 Mar 2018 20:04:10 +0000 Subject: [PATCH 035/121] Adding a UI for choosing cc and cxx compiler --- build.sh | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/build.sh b/build.sh index de43d9f938..cb454955dd 100755 --- a/build.sh +++ b/build.sh @@ -29,17 +29,22 @@ if [ ! -d "./cmake_build" ]; then fi -# set up paths of clang compiler -if [ "$(uname)" == "Darwin" ]; then - CMAKE="$(greadlink -f cmake_build/bin/cmake)" - - export CC=/usr/local/opt/llvm\@3.9/bin/clang - export CXX=/usr/local/opt/llvm\@3.9/bin/clang++ +# set up paths of cc and cxx compiler +if [ "$1" == "gcc" ]; then + export CC="gcc" + export CXX="g++" else - CMAKE="$(readlink -f cmake_build/bin/cmake)" + if [ "$(uname)" == "Darwin" ]; then + CMAKE="$(greadlink -f cmake_build/bin/cmake)" + + export CC=/usr/local/opt/llvm\@3.9/bin/clang + export CXX=/usr/local/opt/llvm\@3.9/bin/clang++ + else + CMAKE="$(readlink -f cmake_build/bin/cmake)" - export CC="clang-3.9" - export CXX="clang++-3.9" + export CC="clang-3.9" + export CXX="clang++-3.9" + fi fi #install EIGEN library From ea946e97611c9e8450ba96ca99658015a3013e7b Mon Sep 17 00:00:00 2001 From: Joerg Reichardt Date: Thu, 29 Mar 2018 16:55:24 +0200 Subject: [PATCH 036/121] Allow to segment skinned and skeletal meshes by their name --- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 31 ++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 9003fca4f7..e2c582430c 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -330,6 +330,27 @@ std::string UAirBlueprintLib::GetMeshName(T* mesh) } } +template<> +std::string UAirBlueprintLib::GetMeshName(USkinnedMeshComponent* mesh) +{ + switch(mesh_naming_method) + { + case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: + if (mesh->GetOwner()) + return std::string(TCHAR_TO_UTF8(*(mesh->GetOwner()->GetName()))); + else + return ""; // std::string(TCHAR_TO_UTF8(*(UKismetSystemLibrary::GetDisplayName(mesh)))); + case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::StaticMeshName: + if (mesh->SkeletalMesh) + return std::string(TCHAR_TO_UTF8(*(mesh->SkeletalMesh->GetName()))); + else + return ""; + default: + return ""; + } +} + + std::string UAirBlueprintLib::GetMeshName(ALandscapeProxy* mesh) { return std::string(TCHAR_TO_UTF8(*(mesh->GetName()))); @@ -341,6 +362,10 @@ void UAirBlueprintLib::InitializeMeshStencilIDs(bool ignore_existing) { InitializeObjectStencilID(*comp, ignore_existing); } + for (TObjectIterator comp; comp; ++comp) + { + InitializeObjectStencilID(*comp, ignore_existing); + } //for (TObjectIterator comp; comp; ++comp) //{ // InitializeObjectStencilID(*comp); @@ -378,6 +403,10 @@ bool UAirBlueprintLib::SetMeshStencilID(const std::string& mesh_name, int object { SetObjectStencilIDIfMatch(*comp, object_id, mesh_name, is_name_regex, name_regex, changes); } + for (TObjectIterator comp; comp; ++comp) + { + SetObjectStencilIDIfMatch(*comp, object_id, mesh_name, is_name_regex, name_regex, changes); + } for (TObjectIterator comp; comp; ++comp) { SetObjectStencilIDIfMatch(*comp, object_id, mesh_name, is_name_regex, name_regex, changes); @@ -552,4 +581,4 @@ float UAirBlueprintLib::GetDisplayGamma() void UAirBlueprintLib::EnableInput(AActor* actor) { actor->EnableInput(actor->GetWorld()->GetFirstPlayerController()); -} \ No newline at end of file +} From d00c7fff655d404e492df8c843e2ddf1a87f0800 Mon Sep 17 00:00:00 2001 From: andrealaffly <36086921+andrealaffly@users.noreply.github.com> Date: Thu, 29 Mar 2018 10:44:00 -0500 Subject: [PATCH 037/121] Add files via upload Adaptive sliding mode control law for quadrotors. This control law guarantees satisfactory trajectory tracking also in case one of the propellers experiences 50% efficiency loss. --- .../firmware/AdaptiveController.hpp | 1371 +++++++++-------- 1 file changed, 713 insertions(+), 658 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 1f3b8baa01..155b7b57f2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -18,664 +18,719 @@ namespace simple_flight { -class AdaptiveController : public IController { -public: - - virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override - { - //TODO: goal is never used! - goal_ = goal; - state_estimator_ = state_estimator; - } - - virtual const Axis4r& getOutput() override - { - // Create vectors to store state variables - Axis3r angle = 0; - Axis3r position = 0; - Axis3r angle_rate = 0; - Axis3r velocity = 0; - - // Assign state variables to placeholder variables - angle = state_estimator_->getAngles(); - angle_rate = state_estimator_->getAngulerVelocity(); - position = state_estimator_->getPosition(); - velocity = state_estimator_->getLinearVelocity(); - - // Send state variables to the adaptive controller - update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], - angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); - - run(); - - // Replace PID control variables with Adaptive control variables - output_controls_[0] = get_U1(); - output_controls_[1] = get_U2(); - output_controls_[2] = get_U3(); - output_controls_[3] = get_U4(); - - return output_controls_; - } - -private: - const IGoal* goal_; - const IStateEstimator* state_estimator_; - Axis4r output_controls_; - -private: - //inertia parameters - const float Ix = 0.02f; - const float Iy = 0.01f; - const float Iz = 0.03f; - - const float l = 0.11f; //arm length, can make more accurate by being specific about lx, ly - const float m = 0.916f; // mass in kg - const float grav = 9.81f; // gravity - const float Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value - - // Static Gain Variables - const float k_phi = -16.75f; // roll angle - const float k_theta = -26.75f; // pitch angle - const float k_psi = -13.0f; // yaw angle - const float k_roll = -450.0f; // roll rate - const float k_pitch = -450.0f; // pitch rate - const float k_yaw = -400.0f; // yaw rate - - //input saturation - const float U1_sat = 1.0f; - const float U2_sat = .95f; - const float U3_sat = .95f; - const float U4_sat = .95f; - - //trajectory parameters - const float pi = 3.14159265359f; - const float period = 45.0f; - const float radius = 2.5f; // input radius of the circle - const float alt = 5.0f; // height used for circle/square - - // other constants - const float SIZE_BUF_FLOAT = 30.0f; - const float NEQN = 7.0f; - -private: - void update_state_vals(float x_val, float vx, float y_val, float vy, float z_val, float vz, float roll, float roll_rate, float pitch, float pitch_rate, float yaw, float yaw_rate) - { - x_in = x_val; - xdot_in = vx; - // negative conventions in the y and z directions are a result of vicon space coordinate needs - y_in = -y_val; - ydot_in = -vy; - z_in = -z_val; - zdot_in = -vz; - phi_in = roll; - P_in = roll_rate; - theta_in = -pitch; - Q_in = -pitch_rate; - psi_in = yaw; - R_in = yaw_rate; - - - // bias modification for level imu implementing deadband - - if (std::abs(phi_in) <= 0.0001) - phi_in = 0; - - if (std::abs(theta_in) <= 0.00001) - theta_in = 0; - - if (std::abs(psi_in) <= 0.0001) - psi_in = 0; - } - - void run() - { - rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); - } - - float get_U1() //pitch - { - return static_cast(U1); - } - - float get_U2() //roll - { - return static_cast(U2); - } - - float get_U3() //thrust - { - return static_cast(U3); - } - - float get_U4() //yaw - { - return static_cast(U4); - } - - /******* Rotate Body velocities to global frame *********/ - void Rotate_uvw_xyz(double u, double v, double w, double phi, double theta, double psi, double result[3][1]) - { - result[0][0] = cos(theta)*cos(psi)*u + (-cos(phi)*sin(theta) + sin(phi)*sin(theta)*cos(phi))*v + (sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi))*w; - result[1][0] = -cos(psi)*u + (cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi))*v + (-sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi))*w; - result[2][0] = -sin(theta)*u + sin(phi)*cos(theta)*v + cos(phi)*cos(theta)*w; - } - void matrix_multiply_4_4_4_1(double mat1[4][4], double mat2[4][1], double result[4][1]) - { - int i, j, k; - double sum; - for (i = 0; i<4; i++) - { - for (j = 0; j<1; j++) - { - sum = 0; - for (k = 0; k<4; k++) - { - sum += mat1[i][k] * mat2[k][j]; - } - result[i][j] = sum; - } - } - } - - - void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) - { - double Fz, xddot, yddot; - int i; - Fz = (zddot + 9.81)*0.9116; - xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); - yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); - if (Fz == 0) { - Fz = 9.81*0.9116; - } - result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; - result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; - // Limit the angle reference values - for (i = 0; i < 2; i++) { - if (result[i][0] > 0.10) { - result[i][0] = 0.10; - } - else if (result[i][0] < -0.10) { - result[i][0] = -0.10; - } - - } - } - - void square_trajectory(double t_val, double result[4][1], double height) - { - t_val = t_val - 50; - if (t_val <= 120) - { - - if (t_val > 0) { - if (t_val < 10) { - result[0][0] = 0; - } - if ((t_val >= 10) && (t_val <= 14)) { - result[0][0] = 2 * (t_val - 10) / size_square; - } - if ((t_val > 14) && (t_val < 18)) { - result[0][0] = 2; - } - if ((t_val >= 18) && (t_val <= 26)) { - result[0][0] = 2 - 2 * (t_val - 18) / size_square; - } - if ((t_val > 26) && (t_val < 34)) { - result[0][0] = -2; - } - if ((t_val >= 34) && (t_val <= 42)) { - result[0][0] = -2 + 2 * (t_val - 34) / size_square; - } - if ((t_val > 42) && (t_val < 50)) { - result[0][0] = 2; - } - if ((t_val >= 50) && (t_val <= 58)) { - result[0][0] = 2 - 2 * (t_val - 50) / size_square; - } - if ((t_val > 58) && (t_val < 66)) { - result[0][0] = -2; - } - if ((t_val >= 66) && (t_val <= 74)) { - result[0][0] = -2 + 2 * (t_val - 66) / size_square; - } - if ((t_val > 74) && (t_val < 82)) { - result[0][0] = 2; - } - if ((t_val >= 82) && (t_val <= 90)) { - result[0][0] = 2 - 2 * (t_val - 82) / size_square; - } - if ((t_val > 90) && (t_val < 98)) { - result[0][0] = -2; - } - if ((t_val >= 98) && (t_val <= 106)) { - result[0][0] = -2 + 2 * (t_val - 98) / size_square; - } - if ((t_val > 106) && (t_val < 114)) { - result[0][0] = 2; - } - } - if (t_val < 10) { - result[1][0] = 0; - } - if ((t_val >= 10) && (t_val <= 14)) { - result[1][0] = 0; - } - if ((t_val > 14) && (t_val < 18)) { - result[1][0] = 2 * (t_val - 14) / size_square; - } - if ((t_val >= 18) && (t_val <= 26)) { - result[1][0] = 2; - } - if ((t_val > 26) && (t_val < 34)) { - result[1][0] = 2 - 2 * (t_val - 26) / size_square; - } - if ((t_val >= 34) && (t_val <= 42)) { - result[1][0] = -2; - } - if ((t_val > 42) && (t_val < 50)) { - result[1][0] = -2 + 2 * (t_val - 42) / size_square; - } - if ((t_val >= 50) && (t_val <= 58)) { - result[1][0] = 2; - } - if ((t_val > 58) && (t_val < 66)) { - result[1][0] = 2 - 2 * (t_val - 58) / size_square; - } - if ((t_val >= 66) && (t_val <= 74)) { - result[1][0] = -2; - } - if ((t_val > 74) && (t_val < 82)) { - result[1][0] = -2 + 2 * (t_val - 74) / size_square; - } - if ((t_val >= 82) && (t_val <= 90)) { - result[1][0] = 2; - } - if ((t_val > 90) && (t_val < 98)) { - result[1][0] = 2 - 2 * (t_val - 90) / size_square; - } - if ((t_val >= 98) && (t_val <= 106)) { - result[1][0] = -2; - } - if ((t_val > 106) && (t_val < 110)) { - result[1][0] = -2 + 2 * (t_val - 106) / size_square; - } - result[0][0] = result[0][0] + 6.91; // add x offset for center of vicon space - result[1][0] = result[1][0] + 4.01; // add y offset for center of vicon space - result[2][0] = height; // z_ref - result[3][0] = 0; // yaw ref - - } - else - { - result[0][0] = 6.91; - result[1][0] = 4.01; - result[2][0] = height; - result[3][0] = 0; - } - t_val = t_val + 50; - - } - - void circle_trajectory(double t_val, double circle_radius, double height, double result[4][1]) - { - if (t_val < 20) { - result[0][0] = 2.5; - result[1][0] = 0; - result[2][0] = height; - result[3][0] = 0; - } - else if (t_val > 20) { - result[0][0] = circle_radius * cos(.22*(t_val - 20)) ; - result[1][0] = circle_radius * sin(.22*(t_val - 20)) ; - result[2][0] = height; - result[3][0] = 0; - } - } - - void step_response_3D(double t_val, double result[4][1]) - { - if (t_val < 30) { - result[0][0] = 0; - result[1][0] = 0; - result[2][0] = 1; - result[3][0] = 0; - } - else if (t_val > 30) { - result[0][0] = 10.5; - result[1][0] = 0; - result[2][0] = 2.5; - result[3][0] = 0; - } - } - - void inverse_3x3(double A[3][3], double result[3][3]) - { - double det_A; // dummy variable - det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); - if (det_A == 0) { - result[0][0] = 0; - result[0][1] = 0; - result[0][2] = 0; - result[1][0] = 0; - result[1][1] = 0; - result[1][2] = 0; - result[2][0] = 0; - result[2][1] = 0; - result[2][2] = 0; - } - else { - result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); - result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); - result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); - result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); - result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); - result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); - result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); - result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); - result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); - } - } - - void PQR_generation(double states[12][1], double result[3][1]) - { - result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; - result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); - result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); - } - - void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) - { - result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); - result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); - result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); - } - - - // --------------------------------------------------------------------------- - // Local Support Functions - // --------------------------------------------------------------------------- - - uint64_t getTimeU() - { - uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); - return last_time_; - } - - - void remapU(double control_u1, double control_u2, double control_u3, double control_u4) - { - // Map to px4 U outputs - U1 = control_u2; // roll - U2 = -control_u3; // pitch - U3 = control_u4; // yaw - U4 = control_u1; // throttle - U_vec[0] = static_cast(U1); - U_vec[1] = static_cast(U2); - U_vec[2] = static_cast(U3); - U_vec[3] = static_cast(U4); - } - - void rungeKutta(float* y, float* yp, uint64_t t_val, float dt, int size) - { - float zero_vec[array_length] = { 0 }; - float k1[array_length] = { 0 }; - float k2[array_length] = { 0 }; - float k3[array_length] = { 0 }; - float k4[array_length] = { 0 }; - float* y_temp; - float* y_out = zero_vec; - y_temp = y; - model(y_temp, static_cast(t_val), y_out); - for (int n = 0; n < size; n++) - { - k1[n] = dt*y_out[n]; - y_temp[n] = y[n] + k1[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k2[n] = dt*y_out[n]; - y_temp[n] = y[n] + k2[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k3[n] = dt*y_out[n]; - y_temp[n] = y[n] + k3[n]; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k4[n] = dt*y_out[n]; - yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; - } - } - - // --------------------------------------------------------------------------- - // SlidingModeModel method - // --------------------------------------------------------------------------- - void model(float* y, float last_time_, float* y_out) - { - unused(last_time_); - - /**********desired trajectory************************/ - - - circle_trajectory(t, radius, alt, refs_temp); - //square_trajectory(t, refs_temp, alt); - //step_response_3D(t, refs_temp); - - x_des = refs_temp[0][0]; - y_des = refs_temp[1][0]; - r[0][0] = 2.5; //z/ - if (t < 10) - { - r[0][0] = 0.25 * t; - } - r[3][0] = 0; //yaw (rad) - - float lambda_theta = 1; - float lambda_theta_rate = 0.06f; - float lambda_phi = 0.5f; - float lambda_phi_rate = 0.05f; - float lambda_psi = 0.4f; - float lambda_psi_rate = 0.05f; - float lambda_z = .5f; - - - /********** Input the State Vector***********/ - x[0][0] = x_in; - x[1][0] = xdot_in; - x[2][0] = y_in; - x[3][0] = ydot_in; - x[4][0] = z_in; - x[5][0] = zdot_in; - x[6][0] = phi_in; - Angles[0][0] = phi_in; - PQR[0][0] = P_in; - x[8][0] = theta_in; - Angles[1][0] = theta_in; - PQR[1][0] = Q_in; - x[10][0] = psi_in; - Angles[2][0] = psi_in; - PQR[2][0] = R_in; - - - Angular_velocities_from_PQR(PQR, Angles, Angular_rates); - x[7][0] = Angular_rates[0][0]; - x[9][0] = Angular_rates[1][0]; - x[11][0] = Angular_rates[2][0]; - - // -4, -10 are adjustable gains for the z controller - zdotdot = -6 * x[5][0] - 15* (x[4][0] - r[0][0]); - - /**********Iconfig Sliding Mode***********************/ - - Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs); - ref_angles[0][0] = refs[0][0]; - ref_angles[1][0] = refs[1][0]; - - - /* Reference angles to be sent to inner loop */ - r[1][0] = ref_angles[0][0]; //phi ref - r[2][0] = ref_angles[1][0]; //theta ref - - /************ Iconfig Control Law *********************/ - // First get integrated uncertainty parameters - delta_z = y[6]; - delta_phi = y[0]; - delta_theta = y[1]; - delta_psi = y[2]; - delta_roll = y[3]; - delta_pitch = y[4]; - delta_yaw = y[5]; - - - U1 = (zdotdot + grav)*m + delta_z; - - y_out[6] = static_cast(lambda_z*zdotdot); // generate sliding surface in z, .015 is adjustable slope for sliding surface - - // error in euler angles - S2_phi = x[6][0] - r[1][0]; - S2_theta = x[8][0] - r[2][0]; - S2_psi = x[10][0] - r[3][0]; - - //cout << x[0][0] << " " << x[1][0] << " " << x[2][0] << " " << x[3][0] << " " << x[4][0] << " " << x[5][0] << " " << x[6][0] << " " << x[7][0] << " " << x[8][0] << " " << x[9][0] << " " << x[10][0] << " " << x[11][0] << " " << "\n"; - - // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired - y_out[0] = static_cast(lambda_phi*S2_phi); - y_out[1] = static_cast(lambda_theta*S2_theta); - y_out[2] = static_cast(lambda_psi*S2_psi); - - R_matrix[0][0] = 1; - R_matrix[1][0] = sin(x[6][0] * tan(x[8][0])); - R_matrix[2][0] = cos(x[6][0])*tan(x[8][0]); - R_matrix[1][0] = 0; - R_matrix[1][1] = cos(x[6][0]); - R_matrix[1][2] = -1 * sin(x[6][0]); - R_matrix[2][0] = 0; - R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]); - R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]); - - inverse_3x3(R_matrix, R_inverse); - rollrate_ref = R_inverse[0][0] * S2_phi*k_phi + R_inverse[0][1] * S2_theta*k_theta + R_inverse[0][2] * S2_psi*k_psi - delta_phi; - pitchrate_ref = R_inverse[1][0] * S2_phi*k_phi + R_inverse[1][1] * S2_theta*k_theta + R_inverse[1][2] * S2_psi*k_psi - delta_theta; - yawrate_ref = R_inverse[2][0] * S2_phi*k_phi + R_inverse[2][1] * S2_theta*k_theta + R_inverse[2][2] * S2_psi*k_psi - delta_psi; - - PQR_generation(x, PQR); - S3_P = PQR[0][0] - rollrate_ref; - S3_Q = PQR[1][0] - pitchrate_ref; - S3_R = PQR[2][0] - yawrate_ref; - - // Sliding surface for the body frame angular rates to be integrated - y_out[3] = static_cast(lambda_phi_rate*S3_P); - y_out[4] = static_cast(lambda_theta_rate*S3_Q); - y_out[5] = static_cast(lambda_psi_rate*S3_R); - - // Calculate controls for roll, pitch, and yaw - U2 = k_roll*S3_P*Ix + (Iz - Iy)*PQR[1][0] * PQR[2][0] - delta_roll; - U3 = k_pitch*S3_Q*Iy + (Ix - Iz)*PQR[0][0] * PQR[2][0] - delta_pitch; - U4 = k_yaw*S3_R*Iz + (Iy - Ix)*PQR[0][0] * PQR[1][0] - delta_yaw; - - - // Rescale such that the outputs normalize from -1,1 - - U1 = sqrt(std::abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal - - U2 = U2 / 80; - - U3 = U3 / 80; - - U4 = U4 / 80; - - - // Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2 - if (U1 > 1) - { - U1 = 1; - } - else if (U1 < 0.35) - { - U1 = 0.35; - } - if (U2 > U2_sat) - { - U2 = U2_sat; - } - else if (U2 < -U2_sat) - { - U2 = -U2_sat; - } - if (U3 > U3_sat) - { - U3 = U3_sat; - } - else if (U3 < -U3_sat) - { - U3 = -U3_sat; - } - if (U4 > U4_sat) - { - U4 = U4_sat; - } - else if (U4 < -U4_sat) - { - U4 = -U4_sat; - } - - remapU(U1, U2, U3, U4); //remap to axis4r - + class AdaptiveController : public IController { + public: + + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + goal_ = goal; + state_estimator_ = state_estimator; + } + + virtual const Axis4r& getOutput() override + { + // Create vectors to store state variables + Axis3r angle = 0; + Axis3r position = 0; + Axis3r angle_rate = 0; + Axis3r velocity = 0; + + // Assign state variables to placeholder variables + angle = state_estimator_->getAngles(); + angle_rate = state_estimator_->getAngulerVelocity(); + position = state_estimator_->getPosition(); + velocity = state_estimator_->getLinearVelocity(); + + // Send state variables to the adaptive controller + update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], + angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); + + update_goals(); + + run(); + + // Replace PID control variables with Adaptive control variables + output_controls_[0] = get_U1(); + output_controls_[1] = get_U2(); + output_controls_[2] = get_U3(); + output_controls_[3] = get_U4(); + + return output_controls_; + } + + private: + const IBoardClock* clock_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + Axis4r output_controls_; + + //inertia parameters + const float Ix = 0.02f; + const float Iy = 0.01f; + const float Iz = 0.03f; + + const float l = 0.11f; //arm length, can make more accurate by being specific about lx, ly + const float m = 0.916f; // mass in kg + const float grav = 9.81f; // gravity + const float Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value + + // Static Gain Variables + const float k_phi = -16.75f; // roll angle //16.75 + const float k_theta = -26.75f; // pitch angle //26.75 + const float k_psi = -1.0f; // yaw angle //13 + const float k_roll = -450.0f; // roll rate //450 + const float k_pitch = -450.0f; // pitch rate //450 + const float k_yaw = -40000.0f; // yaw rate //400 + + //input saturation + const float U1_sat = 1.0f; + const float U2_sat = .95f; + const float U3_sat = .95f; + const float U4_sat = .95f; + + //trajectory parameters + const float pi = 3.14159265359f; + const float period = 45.0f; + const float radius = 2.5f; // input radius of the circle + const float alt = 5.0f; // height used for circle/square + + // other constants + const float SIZE_BUF_FLOAT = 30.0f; + const float NEQN = 7.0f; + + + bool reset = true; + float x_0[12]; + GoalMode last_mode_; + //float error[3] = { 0 }; + float ref_vec[10][3] = { 0 }; + float ref_sum[3] = { 0 }; + float velocity_integrator[3] = { 0 }; + static constexpr int array_length = 7; + float zero[array_length] = { 0 }; + float* adaptive_y = zero; + float* adaptive_output = zero; + float last_yaw = 0.0f; + + //********************** SlidingModeModel Variables ******************************************/ + + // state values + float x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; + + double x_des; + double y_des; + + + // State Vector + double x[12][1]; + double reference[12] = { 0 }; + + // References and trajectory values + double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs + double size_square = 4; // one side of the square is size_square/2 + double r[4][1]; // z, phi, theta, psi references into controller + + // update for angle states in SlidingModeModel + double PQR[3][1], Angles[3][1], Angular_rates[3][1]; + double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc + double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters + double S3_P, S3_Q, S3_R; //error in body frame angular rates + + + // Iconfig Adaptive Sliding Variables + double S2_phi, S2_theta, S2_psi; // error in euler angles + double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration + double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles + double R_matrix[3][3], R_inverse[3][3]; + + // Iconfig Sliding Variables + double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control + + Axis4r U_vec = 0; + + double U1, U2, U3, U4; + + + void update_state_vals(float x_val, float vx, float y_val, float vy, float z_val, float vz, float roll, float roll_rate, float pitch, float pitch_rate, float yaw, float yaw_rate) + { + x_in = x_val; + xdot_in = vx; + y_in = -y_val; + ydot_in = -vy; + z_in = -z_val; + zdot_in = -vz; + phi_in = roll; + P_in = roll_rate; + theta_in = -pitch; + Q_in = -pitch_rate; + psi_in = yaw; + R_in = yaw_rate; + + + // bias modification for level imu implementing deadband + + if (abs(phi_in) <= 0.0001) + phi_in = 0; + + if (abs(theta_in) <= 0.00001) + theta_in = 0; + + if (abs(psi_in) <= 0.0001) + psi_in = 0; + } + + void update_goals() + { + const auto& mode = goal_->getGoalMode(); + const auto& value = goal_->getGoalValue(); + + for (int i = 0; i < 12; i++) + { + reference[i] = 10001.0f; + } + int count1 = 0, count2 = 0, count3 = 0, count4 = 0; + + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) + { + switch (mode[axis]) + { + case GoalModeType::AngleRate: + + switch (count1) + { + case 0: + reference[11] = value[axis]; + break; + case 1: + reference[9] = value[axis]; + break; + case 2: + reference[10] = value[axis]; + break; + } + count1++; + break; + case GoalModeType::AngleLevel: + + switch (count2) + { + case 0: + if (axis == 2) + { + reference[8] = value[axis]; + } + else + { + reference[6] = value[axis]; + } + break; + case 1: + reference[7] = -value[axis]; + break; + case 2: + reference[8] = value[axis]; + break; + } + count2++; + break; + case GoalModeType::VelocityWorld: + + switch (count3) + { + case 0: + reference[3] = value[axis]; + break; + case 1: + reference[4] = value[axis]; + break; + case 2: + reference[5] = value[axis]; + break; + } + count3++; + break; + case GoalModeType::PositionWorld: + + switch (count4) + { + case 0: + if (axis != 0) + { + reference[2] = value[axis]; + } + else + { + reference[0] = value[axis]; + } + break; + case 1: + reference[1] = value[axis]; + break; + case 2: + reference[2] = value[axis]; + break; + } + count4++; + break; + default: + + break; + + + } + if (mode[axis] != last_mode_[axis]) + { + reset = true; + } + last_mode_[axis] = mode[axis]; + } + if (reference[8] < 10000) + { + last_yaw = reference[8]; + } + } + + void run() + { + rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); + } + + float get_U1() //pitch + { + return static_cast(U1); + } + + float get_U2() //roll + { + return static_cast(U2); + } + + float get_U3() //thrust + { + return static_cast(U3); + } + + float get_U4() //yaw + { + return static_cast(U4); + } + + void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) + { + double Fz, xddot, yddot; + int i; + Fz = (zddot + 9.81)*0.9116; + xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); + yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); + if (Fz == 0) { + Fz = 9.81*0.9116; + } + result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; + result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; + // Limit the angle reference values + for (i = 0; i < 2; i++) { + if (result[i][0] > 0.3) { + result[i][0] = 0.3; + } + else if (result[i][0] < -0.3) { + result[i][0] = -0.3; + } + + } + } + + void inverse_3x3(double A[3][3], double result[3][3]) + { + double det_A; // dummy variable + det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + if (det_A == 0) { + result[0][0] = 0; + result[0][1] = 0; + result[0][2] = 0; + result[1][0] = 0; + result[1][1] = 0; + result[1][2] = 0; + result[2][0] = 0; + result[2][1] = 0; + result[2][2] = 0; + } + else { + result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); + result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); + result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); + result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); + result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); + result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); + result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); + result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); + result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); + } + } + + void PQR_generation(double states[12][1], double result[3][1]) + { + result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; + result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); + result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); + } + + void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) + { + result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); + result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); + result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); + } + + + // --------------------------------------------------------------------------- + // Local Support Functions + // --------------------------------------------------------------------------- + + uint64_t getTimeU() + { + uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); + return last_time_; + } + + + void remapU(double control_u1, double control_u2, double control_u3, double control_u4) + { + // Map to px4 U outputs + U1 = control_u2; // roll + U2 = -control_u3; // pitch + U3 = control_u4; // yaw + U4 = control_u1; // throttle + U_vec[0] = static_cast(U1); + U_vec[1] = static_cast(U2); + U_vec[2] = static_cast(U3); + U_vec[3] = static_cast(U4); + } + + void rungeKutta(float* y, float* yp, uint64_t t_val, float dt, int size) + { + float zero_vec[array_length] = { 0 }; + float k1[array_length] = { 0 }; + float k2[array_length] = { 0 }; + float k3[array_length] = { 0 }; + float k4[array_length] = { 0 }; + float* y_temp; + float* y_out = zero_vec; + y = yp; + y_temp = y; + model(y_temp, static_cast(t_val), y_out); + for (int n = 0; n < size; n++) + { + k1[n] = dt*y_out[n]; + y_temp[n] = y[n] + k1[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k2[n] = dt*y_out[n]; + y_temp[n] = y[n] + k2[n] / 2; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k3[n] = dt*y_out[n]; + y_temp[n] = y[n] + k3[n]; + } + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) + { + k4[n] = dt*y_out[n]; + yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; + } + } + + // --------------------------------------------------------------------------- + // SlidingModeModel method + // --------------------------------------------------------------------------- + void model(float* y, float last_time_, float* y_out) + { + unused(last_time_); + + /********** Input the State Vector***********/ + x[0][0] = x_in; + x[1][0] = xdot_in; + x[2][0] = y_in; + x[3][0] = ydot_in; + x[4][0] = z_in; + x[5][0] = zdot_in; + x[6][0] = phi_in; + PQR[0][0] = P_in; + x[8][0] = theta_in; + PQR[1][0] = Q_in; + x[10][0] = -psi_in; + PQR[2][0] = R_in; + + if (reset) + { + for (int i = 0; i < 12; i++) + { + x_0[i] = x[i][0]; + } + velocity_integrator[2] = 0.0f; + reset = false; + } + + float velocity_real[3] = { x[1][0],x[3][0],x[5][0] }; + float velocity_goal[3] = { reference[4],reference[3],reference[5] }; + + for (int i = 0; i < 3; i++) + { + ref_sum[i] = 0; + for (int j = 0; j < 9; j++) + { + ref_vec[j][i] = ref_vec[j + 1][i]; + } + if (abs(velocity_goal[i]) < 10000) + { + if (i == 0) + { + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.004f; + ref_vec[9][i] = -x[2 * i][0] - velocity_integrator[i]; + } + if (i == 1) + { + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0007f; + ref_vec[9][i] = x[2 * i][0] - velocity_integrator[i]; + } + if (i == 2) + { + velocity_integrator[i] += (-velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0005f; + ref_vec[9][i] = (x[2 * i][0] + velocity_integrator[i]); + } + } + if (abs(velocity_goal[i]) > 10000) + { + ref_vec[9][i] = reference[i]; + if (i == 0) + { + ref_vec[9][0] = reference[1]; + } + else if (i == 1) + { + ref_vec[9][1] = reference[0]; + } + if (i == 2) + { + ref_vec[9][i] = -ref_vec[9][i]; + } + if (abs(ref_vec[9][i]) > 10000) + { + ref_vec[9][i] = x[2 * i][0]; + if (i == 0) + { + ref_vec[9][i] = x[0][0]; + } + if (i == 1) + { + ref_vec[9][i] = x[2][0]; + } + } + } + + if (i > 0) + { + ref_vec[9][i] = -ref_vec[9][i]; + } + for (int j = 0; j < 10; j++) + { + ref_sum[i] += ref_vec[j][i]; + } + ref_sum[i] = ref_sum[i] /= 10;//ref_vec[9][i];// + } + + x_des = ref_sum[0]; + if (x_des > 10000) + { + x_des = x[0][0]; + } + + y_des = ref_sum[1]; + if (y_des > 10000) + { + y_des = x[2][0]; + } + + r[0][0] = -ref_sum[2]; + if (abs(r[0][0]) > 10000) + { + r[0][0] = x[4][0]; + } + + r[3][0] = reference[8]; + if (r[3][0] > 10000) + { + r[3][0] =-last_yaw; + } + + + float lambda_theta = 0.5f; + float lambda_theta_rate = 0.5f; + float lambda_phi = 0.95f; + float lambda_phi_rate = 0.95f; + float lambda_psi = 0.0004f; + float lambda_psi_rate = 0.05f; + float lambda_z = 0.5f; + + + Angular_velocities_from_PQR(PQR, Angles, Angular_rates); + x[7][0] = Angular_rates[0][0]; + x[9][0] = Angular_rates[1][0]; + x[11][0] = Angular_rates[2][0]; + + // -9, -21 are adjustable gains for the z controller + zdotdot = -9 * x[5][0] - 21 * (x[4][0] - r[0][0]); + + /**********Iconfig Sliding Mode***********************/ + + Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs); + ref_angles[0][0] = refs[0][0]; + ref_angles[1][0] = refs[1][0]; + + + /* Reference angles to be sent to inner loop */ + r[1][0] = ref_angles[0][0]; //phi ref + if (reference[6] < 10000 && reference[6] != 0.0f) + { + r[1][0] = reference[6]; + } + r[2][0] = ref_angles[1][0]; //theta ref + if (reference[7] < 10000 && reference[7] != 0.0f) + { + r[2][0] = reference[7]; + } + + /************ Iconfig Control Law *********************/ + // First get integrated uncertainty parameters + delta_z = y[6]; + delta_phi = y[0]; + delta_theta = y[1]; + delta_psi = y[2]; + delta_roll = y[3]; + delta_pitch = y[4]; + delta_yaw = y[5]; + + + U1 = (zdotdot + grav)*m + delta_z; + + y_out[6] = lambda_z*zdotdot; // generate sliding surface in z, .015 is adjustable slope for sliding surface + + // error in euler angles + S2_phi = x[6][0] - r[1][0]; + S2_theta = x[8][0] - r[2][0]; + S2_psi = -x[10][0] - r[3][0]; + if (S2_psi > PI) + { + S2_psi -= 2*PI; + } + if (S2_psi < -PI) + { + S2_psi += 2 * PI; + } + + // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired + y_out[0] = lambda_phi*S2_phi; + y_out[1] = lambda_theta*S2_theta; + y_out[2] = lambda_psi*S2_psi; + + R_matrix[0][0] = 1; + R_matrix[1][0] = sin(x[6][0] * tan(x[8][0])); + R_matrix[2][0] = cos(x[6][0])*tan(x[8][0]); + R_matrix[1][0] = 0; + R_matrix[1][1] = cos(x[6][0]); + R_matrix[1][2] = -1 * sin(x[6][0]); + R_matrix[2][0] = 0; + R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]); + R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]); + + inverse_3x3(R_matrix, R_inverse); + rollrate_ref = R_inverse[0][0] * S2_phi*k_phi + R_inverse[0][1] * S2_theta*k_theta + R_inverse[0][2] * S2_psi*k_psi - delta_phi; + pitchrate_ref = R_inverse[1][0] * S2_phi*k_phi + R_inverse[1][1] * S2_theta*k_theta + R_inverse[1][2] * S2_psi*k_psi - delta_theta; + yawrate_ref = R_inverse[2][0] * S2_phi*k_phi + R_inverse[2][1] * S2_theta*k_theta + R_inverse[2][2] * S2_psi*k_psi - delta_psi; + + if (reference[9] < 10000 && reference[9] != 0.0f) + { + rollrate_ref = reference[9]; + } + if (reference[10] < 10000 && reference[10] != 0.0f) + { + pitchrate_ref = reference[10]; + } + if (reference[11] < 10000) + { + yawrate_ref = reference[11]; + } + + PQR_generation(x, PQR); + S3_P = PQR[0][0] - rollrate_ref; + S3_Q = PQR[1][0] - pitchrate_ref; + S3_R = PQR[2][0] - yawrate_ref; + + // Sliding surface for the body frame angular rates to be integrated + y_out[3] = lambda_phi_rate*S3_P; + y_out[4] = lambda_theta_rate*S3_Q; + y_out[5] = lambda_psi_rate*S3_R; + + // Calculate controls for roll, pitch, and yaw + U2 = k_roll*S3_P*Ix + (Iz - Iy)*PQR[1][0] * PQR[2][0] - delta_roll; + U3 = k_pitch*S3_Q*Iy + (Ix - Iz)*PQR[0][0] * PQR[2][0] - delta_pitch; + U4 = k_yaw*S3_R*Iz + (Iy - Ix)*PQR[0][0] * PQR[1][0] - delta_yaw; + + + // Rescale such that the outputs normalize from -1,1 + + U1 = U1 / 80;//sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal + + U2 = U2 / 80; + + U3 = U3 / 80; + + U4 = U4 / 80; + + + // Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2 + if (U1 > 1) + { + U1 = 1; + } + else if (U1 < 0.35) + { + U1 = 0.35; + } + if (U2 > U2_sat) + { + U2 = U2_sat; + } + else if (U2 < -U2_sat) + { + U2 = -U2_sat; + } + if (U3 > U3_sat) + { + U3 = U3_sat; + } + else if (U3 < -U3_sat) + { + U3 = -U3_sat; + } + if (U4 > U4_sat) + { + U4 = U4_sat; + } + else if (U4 < -U4_sat) + { + U4 = -U4_sat; + } + + remapU(U1, U2, U3, U4); //remap to axis4r + + } // SlidingModeModel */ + + }; - } // SlidingModeModel */ - -private: - const IBoardClock* clock_; - static constexpr int array_length = 7; - float zero[array_length] = { 0 }; - float* adaptive_y = zero; - float* adaptive_output = zero; - - //********************** SlidingModeModel Variables ******************************************/ - - // state values - float x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; - - double x_des; - double y_des; - - // State Vector - double x[12][1]; - - // References and trajectory values - double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs - double size_square = 4; // one side of the square is size_square/2 - double r[4][1]; // z, phi, theta, psi references into controller - - // update for angle states in SlidingModeModel - double PQR[3][1], Angles[3][1], Angular_rates[3][1]; - double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc - double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters - double S3_P, S3_Q, S3_R; //error in body frame angular rates - - - // Iconfig Adaptive Sliding Variables - double S2_phi, S2_theta, S2_psi; // error in euler angles - double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration - double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles - double R_matrix[3][3], R_inverse[3][3]; - - // Iconfig Sliding Variables - double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control - - Axis4r U_vec = 0; - - double U1, U2, U3, U4; - - double t = 0.0; //t variable - -}; - } -#endif +#endif \ No newline at end of file From 50b3d0aaebc9c1cdbaef1e1021381064976463e5 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 29 Mar 2018 16:33:15 -0700 Subject: [PATCH 038/121] tab changes, float -> double --- .../firmware/AdaptiveController.hpp | 1221 ++++++++--------- 1 file changed, 610 insertions(+), 611 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 155b7b57f2..53881b593f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -18,718 +18,717 @@ namespace simple_flight { - class AdaptiveController : public IController { - public: +class AdaptiveController : public IController { +public: + + virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override + { + goal_ = goal; + state_estimator_ = state_estimator; + } + + virtual const Axis4r& getOutput() override + { + // Create vectors to store state variables + Axis3r angle = 0; + Axis3r position = 0; + Axis3r angle_rate = 0; + Axis3r velocity = 0; + + // Assign state variables to placeholder variables + angle = state_estimator_->getAngles(); + angle_rate = state_estimator_->getAngulerVelocity(); + position = state_estimator_->getPosition(); + velocity = state_estimator_->getLinearVelocity(); + + // Send state variables to the adaptive controller + update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], + angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); + + update_goals(); + + run(); + + // Replace PID control variables with Adaptive control variables + output_controls_[0] = get_U1(); + output_controls_[1] = get_U2(); + output_controls_[2] = get_U3(); + output_controls_[3] = get_U4(); + + return output_controls_; + } + +private: + const IBoardClock* clock_; + const IGoal* goal_; + const IStateEstimator* state_estimator_; + Axis4r output_controls_; + + //inertia parameters + const double Ix = 0.02f; + const double Iy = 0.01f; + const double Iz = 0.03f; + + const double l = 0.11f; //arm length, can make more accurate by being specific about lx, ly + const double m = 0.916f; // mass in kg + const double grav = 9.81f; // gravity + const double Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value + + // Static Gain Variables + const double k_phi = -16.75f; // roll angle //16.75 + const double k_theta = -26.75f; // pitch angle //26.75 + const double k_psi = -1.0f; // yaw angle //13 + const double k_roll = -450.0f; // roll rate //450 + const double k_pitch = -450.0f; // pitch rate //450 + const double k_yaw = -40000.0f; // yaw rate //400 + + //input saturation + const double U1_sat = 1.0f; + const double U2_sat = .95f; + const double U3_sat = .95f; + const double U4_sat = .95f; + + //trajectory parameters + const double pi = 3.14159265359f; + const double period = 45.0f; + const double radius = 2.5f; // input radius of the circle + const double alt = 5.0f; // height used for circle/square + + // other constants + const double NEQN = 7.0f; + + + bool reset = true; + double x_0[12]; + GoalMode last_mode_; + //double error[3] = { 0 }; + double ref_vec[10][3] = { 0 }; + double ref_sum[3] = { 0 }; + double velocity_integrator[3] = { 0 }; + static constexpr int array_length = 7; + double zero[array_length] = { 0 }; + double* adaptive_y = zero; + double* adaptive_output = zero; + double last_yaw = 0.0f; + + //********************** SlidingModeModel Variables ******************************************/ + + // state values + double x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; + + double x_des; + double y_des; + + + // State Vector + double x[12][1]; + double reference[12] = { 0 }; + + // References and trajectory values + double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs + double size_square = 4; // one side of the square is size_square/2 + double r[4][1]; // z, phi, theta, psi references into controller + + // update for angle states in SlidingModeModel + double PQR[3][1], Angles[3][1], Angular_rates[3][1]; + double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc + double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters + double S3_P, S3_Q, S3_R; //error in body frame angular rates + + + // Iconfig Adaptive Sliding Variables + double S2_phi, S2_theta, S2_psi; // error in euler angles + double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration + double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles + double R_matrix[3][3], R_inverse[3][3]; + + // Iconfig Sliding Variables + double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control + + Axis4r U_vec = 0; + + double U1, U2, U3, U4; + + + void update_state_vals(double x_val, double vx, double y_val, double vy, double z_val, double vz, double roll, double roll_rate, double pitch, double pitch_rate, double yaw, double yaw_rate) + { + x_in = x_val; + xdot_in = vx; + y_in = -y_val; + ydot_in = -vy; + z_in = -z_val; + zdot_in = -vz; + phi_in = roll; + P_in = roll_rate; + theta_in = -pitch; + Q_in = -pitch_rate; + psi_in = yaw; + R_in = yaw_rate; + + + // bias modification for level imu implementing deadband + + if (abs(phi_in) <= 0.0001) + phi_in = 0; + + if (abs(theta_in) <= 0.00001) + theta_in = 0; - virtual void initialize(const IGoal* goal, const IStateEstimator* state_estimator) override - { - goal_ = goal; - state_estimator_ = state_estimator; - } - - virtual const Axis4r& getOutput() override - { - // Create vectors to store state variables - Axis3r angle = 0; - Axis3r position = 0; - Axis3r angle_rate = 0; - Axis3r velocity = 0; - - // Assign state variables to placeholder variables - angle = state_estimator_->getAngles(); - angle_rate = state_estimator_->getAngulerVelocity(); - position = state_estimator_->getPosition(); - velocity = state_estimator_->getLinearVelocity(); - - // Send state variables to the adaptive controller - update_state_vals(position[0], velocity[0], position[1], velocity[1], position[2], velocity[2], - angle[0], angle_rate[0], angle[1], angle_rate[1], angle[2], angle_rate[2]); - - update_goals(); - - run(); - - // Replace PID control variables with Adaptive control variables - output_controls_[0] = get_U1(); - output_controls_[1] = get_U2(); - output_controls_[2] = get_U3(); - output_controls_[3] = get_U4(); - - return output_controls_; - } - - private: - const IBoardClock* clock_; - const IGoal* goal_; - const IStateEstimator* state_estimator_; - Axis4r output_controls_; - - //inertia parameters - const float Ix = 0.02f; - const float Iy = 0.01f; - const float Iz = 0.03f; - - const float l = 0.11f; //arm length, can make more accurate by being specific about lx, ly - const float m = 0.916f; // mass in kg - const float grav = 9.81f; // gravity - const float Jr = 0.00006f; // inertia of rotor, currently an estimate; make more accurate by getting a measured value - - // Static Gain Variables - const float k_phi = -16.75f; // roll angle //16.75 - const float k_theta = -26.75f; // pitch angle //26.75 - const float k_psi = -1.0f; // yaw angle //13 - const float k_roll = -450.0f; // roll rate //450 - const float k_pitch = -450.0f; // pitch rate //450 - const float k_yaw = -40000.0f; // yaw rate //400 - - //input saturation - const float U1_sat = 1.0f; - const float U2_sat = .95f; - const float U3_sat = .95f; - const float U4_sat = .95f; - - //trajectory parameters - const float pi = 3.14159265359f; - const float period = 45.0f; - const float radius = 2.5f; // input radius of the circle - const float alt = 5.0f; // height used for circle/square - - // other constants - const float SIZE_BUF_FLOAT = 30.0f; - const float NEQN = 7.0f; - - - bool reset = true; - float x_0[12]; - GoalMode last_mode_; - //float error[3] = { 0 }; - float ref_vec[10][3] = { 0 }; - float ref_sum[3] = { 0 }; - float velocity_integrator[3] = { 0 }; - static constexpr int array_length = 7; - float zero[array_length] = { 0 }; - float* adaptive_y = zero; - float* adaptive_output = zero; - float last_yaw = 0.0f; - - //********************** SlidingModeModel Variables ******************************************/ - - // state values - float x_in, xdot_in, y_in, ydot_in, z_in, zdot_in, phi_in, P_in, theta_in, Q_in, psi_in, R_in; - - double x_des; - double y_des; - - - // State Vector - double x[12][1]; - double reference[12] = { 0 }; - - // References and trajectory values - double refs_temp[4][1]; //temp vector for storing x,y,z,yaw refs - double size_square = 4; // one side of the square is size_square/2 - double r[4][1]; // z, phi, theta, psi references into controller - - // update for angle states in SlidingModeModel - double PQR[3][1], Angles[3][1], Angular_rates[3][1]; - double rollrate_ref, pitchrate_ref, yawrate_ref; //pc, qc, rc - double delta_roll, delta_pitch, delta_yaw; // uncertainty parameters - double S3_P, S3_Q, S3_R; //error in body frame angular rates - - - // Iconfig Adaptive Sliding Variables - double S2_phi, S2_theta, S2_psi; // error in euler angles - double delta_z, zdotdot; // uncertainty in z and calculated desired acceleration - double delta_phi, delta_theta, delta_psi; // uncertainty in euler angles - double R_matrix[3][3], R_inverse[3][3]; - - // Iconfig Sliding Variables - double refs[2][1], ref_angles[2][1]; // reference angles output from outer loop control - - Axis4r U_vec = 0; + if (abs(psi_in) <= 0.0001) + psi_in = 0; + } - double U1, U2, U3, U4; + void update_goals() + { + const auto& mode = goal_->getGoalMode(); + const auto& value = goal_->getGoalValue(); - - void update_state_vals(float x_val, float vx, float y_val, float vy, float z_val, float vz, float roll, float roll_rate, float pitch, float pitch_rate, float yaw, float yaw_rate) + for (int i = 0; i < 12; i++) { - x_in = x_val; - xdot_in = vx; - y_in = -y_val; - ydot_in = -vy; - z_in = -z_val; - zdot_in = -vz; - phi_in = roll; - P_in = roll_rate; - theta_in = -pitch; - Q_in = -pitch_rate; - psi_in = yaw; - R_in = yaw_rate; - - - // bias modification for level imu implementing deadband - - if (abs(phi_in) <= 0.0001) - phi_in = 0; - - if (abs(theta_in) <= 0.00001) - theta_in = 0; - - if (abs(psi_in) <= 0.0001) - psi_in = 0; + reference[i] = 10001.0f; } + int count1 = 0, count2 = 0, count3 = 0, count4 = 0; - void update_goals() - { - const auto& mode = goal_->getGoalMode(); - const auto& value = goal_->getGoalValue(); - - for (int i = 0; i < 12; i++) + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) + { + switch (mode[axis]) { - reference[i] = 10001.0f; - } - int count1 = 0, count2 = 0, count3 = 0, count4 = 0; - - for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) - { - switch (mode[axis]) - { - case GoalModeType::AngleRate: + case GoalModeType::AngleRate: - switch (count1) - { - case 0: - reference[11] = value[axis]; - break; - case 1: - reference[9] = value[axis]; - break; - case 2: - reference[10] = value[axis]; - break; - } - count1++; + switch (count1) + { + case 0: + reference[11] = value[axis]; + break; + case 1: + reference[9] = value[axis]; + break; + case 2: + reference[10] = value[axis]; break; - case GoalModeType::AngleLevel: + } + count1++; + break; + case GoalModeType::AngleLevel: - switch (count2) + switch (count2) + { + case 0: + if (axis == 2) { - case 0: - if (axis == 2) - { - reference[8] = value[axis]; - } - else - { - reference[6] = value[axis]; - } - break; - case 1: - reference[7] = -value[axis]; - break; - case 2: reference[8] = value[axis]; - break; } - count2++; - break; - case GoalModeType::VelocityWorld: - - switch (count3) + else { - case 0: - reference[3] = value[axis]; - break; - case 1: - reference[4] = value[axis]; - break; - case 2: - reference[5] = value[axis]; - break; + reference[6] = value[axis]; } - count3++; break; - case GoalModeType::PositionWorld: + case 1: + reference[7] = -value[axis]; + break; + case 2: + reference[8] = value[axis]; + break; + } + count2++; + break; + case GoalModeType::VelocityWorld: + + switch (count3) + { + case 0: + reference[3] = value[axis]; + break; + case 1: + reference[4] = value[axis]; + break; + case 2: + reference[5] = value[axis]; + break; + } + count3++; + break; + case GoalModeType::PositionWorld: - switch (count4) + switch (count4) + { + case 0: + if (axis != 0) { - case 0: - if (axis != 0) - { - reference[2] = value[axis]; - } - else - { - reference[0] = value[axis]; - } - break; - case 1: - reference[1] = value[axis]; - break; - case 2: reference[2] = value[axis]; - break; } - count4++; - break; - default: + else + { + reference[0] = value[axis]; + } + break; + case 1: + reference[1] = value[axis]; + break; + case 2: + reference[2] = value[axis]; + break; + } + count4++; + break; + default: - break; + break; - } - if (mode[axis] != last_mode_[axis]) - { - reset = true; - } - last_mode_[axis] = mode[axis]; } - if (reference[8] < 10000) + if (mode[axis] != last_mode_[axis]) { - last_yaw = reference[8]; + reset = true; } + last_mode_[axis] = mode[axis]; } - - void run() + if (reference[8] < 10000) { - rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); + last_yaw = reference[8]; } - - float get_U1() //pitch - { - return static_cast(U1); + } + + void run() + { + rungeKutta(adaptive_y, adaptive_output, getTimeU(), 0.003f, array_length); + } + + float get_U1() //pitch + { + return static_cast(U1); + } + + float get_U2() //roll + { + return static_cast(U2); + } + + float get_U3() //thrust + { + return static_cast(U3); + } + + float get_U4() //yaw + { + return static_cast(U4); + } + + void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) + { + double Fz, xddot, yddot; + int i; + Fz = (zddot + 9.81)*0.9116; + xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); + yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); + if (Fz == 0) { + Fz = 9.81*0.9116; } + result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; + result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; + // Limit the angle reference values + for (i = 0; i < 2; i++) { + if (result[i][0] > 0.3) { + result[i][0] = 0.3; + } + else if (result[i][0] < -0.3) { + result[i][0] = -0.3; + } - float get_U2() //roll - { - return static_cast(U2); } - - float get_U3() //thrust - { - return static_cast(U3); + } + + void inverse_3x3(double A[3][3], double result[3][3]) + { + double det_A; // dummy variable + det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); + if (det_A == 0) { + result[0][0] = 0; + result[0][1] = 0; + result[0][2] = 0; + result[1][0] = 0; + result[1][1] = 0; + result[1][2] = 0; + result[2][0] = 0; + result[2][1] = 0; + result[2][2] = 0; } - - float get_U4() //yaw - { - return static_cast(U4); + else { + result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); + result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); + result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); + result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); + result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); + result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); + result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); + result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); + result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); } - - void Sliding_Iconfig(double zddot, double x_desired, double x_current, double xdot, double y_desired, double y_current, double ydot, double yaw_current, double result[2][1]) + } + + void PQR_generation(double states[12][1], double result[3][1]) + { + result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; + result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); + result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); + } + + void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) + { + result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); + result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); + result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); + } + + + // --------------------------------------------------------------------------- + // Local Support Functions + // --------------------------------------------------------------------------- + + uint64_t getTimeU() + { + uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); + return last_time_; + } + + + void remapU(double control_u1, double control_u2, double control_u3, double control_u4) + { + // Map to px4 U outputs + U1 = control_u2; // roll + U2 = -control_u3; // pitch + U3 = control_u4; // yaw + U4 = control_u1; // throttle + U_vec[0] = static_cast(U1); + U_vec[1] = static_cast(U2); + U_vec[2] = static_cast(U3); + U_vec[3] = static_cast(U4); + } + + void rungeKutta(double* y, double* yp, uint64_t t_val, double dt, int size) + { + double zero_vec[array_length] = { 0 }; + double k1[array_length] = { 0 }; + double k2[array_length] = { 0 }; + double k3[array_length] = { 0 }; + double k4[array_length] = { 0 }; + double* y_temp; + double* y_out = zero_vec; + y = yp; + y_temp = y; + model(y_temp, static_cast(t_val), y_out); + for (int n = 0; n < size; n++) { - double Fz, xddot, yddot; - int i; - Fz = (zddot + 9.81)*0.9116; - xddot = -2.0 * xdot - 2.0 * (x_current - x_desired); - yddot = -2.0 * ydot - 3.0 * (y_current - y_desired); - if (Fz == 0) { - Fz = 9.81*0.9116; - } - result[0][0] = (xddot*sin(yaw_current) - yddot*cos(yaw_current))*0.9116 / Fz; - result[1][0] = (xddot*cos(yaw_current) + yddot*sin(yaw_current))*0.9116 / Fz; - // Limit the angle reference values - for (i = 0; i < 2; i++) { - if (result[i][0] > 0.3) { - result[i][0] = 0.3; - } - else if (result[i][0] < -0.3) { - result[i][0] = -0.3; - } - - } + k1[n] = dt*y_out[n]; + y_temp[n] = y[n] + k1[n] / 2; } - - void inverse_3x3(double A[3][3], double result[3][3]) + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { - double det_A; // dummy variable - det_A = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) + A[0][1] * (A[1][2] * A[2][0] - A[1][0] * A[2][2])*A[2][0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]); - if (det_A == 0) { - result[0][0] = 0; - result[0][1] = 0; - result[0][2] = 0; - result[1][0] = 0; - result[1][1] = 0; - result[1][2] = 0; - result[2][0] = 0; - result[2][1] = 0; - result[2][2] = 0; - } - else { - result[0][0] = (1 / det_A)*(A[1][1] * A[2][2] - A[1][2] * A[2][1]); - result[0][1] = (1 / det_A)*(A[0][2] * A[2][1] - A[0][1] * A[2][2]); - result[0][2] = (1 / det_A)*(A[0][1] * A[1][2] - A[0][2] * A[1][1]); - result[1][0] = (1 / det_A)*(A[1][2] * A[2][0] - A[1][0] * A[2][2]); - result[1][1] = (1 / det_A)*(A[0][0] * A[2][2] - A[0][2] * A[2][0]); - result[1][2] = (1 / det_A)*(A[0][2] * A[1][0] - A[0][0] * A[1][2]); - result[2][0] = (1 / det_A)*(A[1][0] * A[2][1] - A[1][1] * A[2][0]); - result[2][1] = (1 / det_A)*(A[0][1] * A[2][0] - A[0][0] * A[2][1]); - result[2][2] = (1 / det_A)*(A[0][0] * A[1][1] - A[0][1] * A[1][0]); - } + k2[n] = dt*y_out[n]; + y_temp[n] = y[n] + k2[n] / 2; } - - void PQR_generation(double states[12][1], double result[3][1]) + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { - result[0][0] = states[7][0] - sin(states[6][0])*states[10][0]; - result[1][0] = states[9][0] * cos(states[6][0]) + states[10][0] * (cos(states[8][0])*sin(states[6][0])); - result[2][0] = states[11][0] * (cos(states[8][0])*cos(states[6][0])) - states[9][0] * sin(states[6][0]); + k3[n] = dt*y_out[n]; + y_temp[n] = y[n] + k3[n]; } - - void Angular_velocities_from_PQR(double PQR_val[3][1], double Angles_val[3][1], double result[3][1]) + model(y_temp, t_val + dt / 2, y_out); + for (int n = 0; n < size; n++) { - result[0][0] = PQR_val[0][0] + PQR_val[1][0] * sin(Angles_val[0][0])*tan(Angles_val[1][0]) + PQR_val[2][0] * cos(Angles_val[0][0])*tan(Angles_val[1][0]); - result[1][0] = PQR_val[1][0] * cos(Angles_val[0][0]) - PQR_val[2][0] * sin(Angles_val[0][0]); - result[2][0] = PQR_val[1][0] * (sin(Angles_val[0][0]) / cos(Angles_val[1][0])) + PQR_val[2][0] * (cos(Angles_val[0][0]) / cos(Angles_val[1][0])); + k4[n] = dt*y_out[n]; + yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; } - - - // --------------------------------------------------------------------------- - // Local Support Functions - // --------------------------------------------------------------------------- - - uint64_t getTimeU() + } + + // --------------------------------------------------------------------------- + // SlidingModeModel method + // --------------------------------------------------------------------------- + void model(double* y, double last_time_, double* y_out) + { + unused(last_time_); + + /********** Input the State Vector***********/ + x[0][0] = x_in; + x[1][0] = xdot_in; + x[2][0] = y_in; + x[3][0] = ydot_in; + x[4][0] = z_in; + x[5][0] = zdot_in; + x[6][0] = phi_in; + PQR[0][0] = P_in; + x[8][0] = theta_in; + PQR[1][0] = Q_in; + x[10][0] = -psi_in; + PQR[2][0] = R_in; + + if (reset) { - uint64_t last_time_ = clock_ == nullptr ? 0 : clock_->millis(); - return last_time_; - } - - - void remapU(double control_u1, double control_u2, double control_u3, double control_u4) - { - // Map to px4 U outputs - U1 = control_u2; // roll - U2 = -control_u3; // pitch - U3 = control_u4; // yaw - U4 = control_u1; // throttle - U_vec[0] = static_cast(U1); - U_vec[1] = static_cast(U2); - U_vec[2] = static_cast(U3); - U_vec[3] = static_cast(U4); - } - - void rungeKutta(float* y, float* yp, uint64_t t_val, float dt, int size) - { - float zero_vec[array_length] = { 0 }; - float k1[array_length] = { 0 }; - float k2[array_length] = { 0 }; - float k3[array_length] = { 0 }; - float k4[array_length] = { 0 }; - float* y_temp; - float* y_out = zero_vec; - y = yp; - y_temp = y; - model(y_temp, static_cast(t_val), y_out); - for (int n = 0; n < size; n++) - { - k1[n] = dt*y_out[n]; - y_temp[n] = y[n] + k1[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k2[n] = dt*y_out[n]; - y_temp[n] = y[n] + k2[n] / 2; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) - { - k3[n] = dt*y_out[n]; - y_temp[n] = y[n] + k3[n]; - } - model(y_temp, t_val + dt / 2, y_out); - for (int n = 0; n < size; n++) + for (int i = 0; i < 12; i++) { - k4[n] = dt*y_out[n]; - yp[n] = y[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; + x_0[i] = x[i][0]; } + velocity_integrator[2] = 0.0f; + reset = false; } - // --------------------------------------------------------------------------- - // SlidingModeModel method - // --------------------------------------------------------------------------- - void model(float* y, float last_time_, float* y_out) + double velocity_real[3] = { x[1][0],x[3][0],x[5][0] }; + double velocity_goal[3] = { reference[4],reference[3],reference[5] }; + + for (int i = 0; i < 3; i++) { - unused(last_time_); - - /********** Input the State Vector***********/ - x[0][0] = x_in; - x[1][0] = xdot_in; - x[2][0] = y_in; - x[3][0] = ydot_in; - x[4][0] = z_in; - x[5][0] = zdot_in; - x[6][0] = phi_in; - PQR[0][0] = P_in; - x[8][0] = theta_in; - PQR[1][0] = Q_in; - x[10][0] = -psi_in; - PQR[2][0] = R_in; - - if (reset) + ref_sum[i] = 0; + for (int j = 0; j < 9; j++) + { + ref_vec[j][i] = ref_vec[j + 1][i]; + } + if (abs(velocity_goal[i]) < 10000) { - for (int i = 0; i < 12; i++) + if (i == 0) + { + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.004f; + ref_vec[9][i] = -x[2 * i][0] - velocity_integrator[i]; + } + if (i == 1) { - x_0[i] = x[i][0]; + velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0007f; + ref_vec[9][i] = x[2 * i][0] - velocity_integrator[i]; + } + if (i == 2) + { + velocity_integrator[i] += (-velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0005f; + ref_vec[9][i] = (x[2 * i][0] + velocity_integrator[i]); } - velocity_integrator[2] = 0.0f; - reset = false; } - - float velocity_real[3] = { x[1][0],x[3][0],x[5][0] }; - float velocity_goal[3] = { reference[4],reference[3],reference[5] }; - - for (int i = 0; i < 3; i++) + if (abs(velocity_goal[i]) > 10000) { - ref_sum[i] = 0; - for (int j = 0; j < 9; j++) + ref_vec[9][i] = reference[i]; + if (i == 0) { - ref_vec[j][i] = ref_vec[j + 1][i]; + ref_vec[9][0] = reference[1]; } - if (abs(velocity_goal[i]) < 10000) + else if (i == 1) { - if (i == 0) - { - velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.004f; - ref_vec[9][i] = -x[2 * i][0] - velocity_integrator[i]; - } - if (i == 1) - { - velocity_integrator[i] += (velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0007f; - ref_vec[9][i] = x[2 * i][0] - velocity_integrator[i]; - } - if (i == 2) - { - velocity_integrator[i] += (-velocity_real[i] - velocity_goal[i] - x_0[2 * i + 1]) * 0.0005f; - ref_vec[9][i] = (x[2 * i][0] + velocity_integrator[i]); - } + ref_vec[9][1] = reference[0]; + } + if (i == 2) + { + ref_vec[9][i] = -ref_vec[9][i]; } - if (abs(velocity_goal[i]) > 10000) + if (abs(ref_vec[9][i]) > 10000) { - ref_vec[9][i] = reference[i]; + ref_vec[9][i] = x[2 * i][0]; if (i == 0) { - ref_vec[9][0] = reference[1]; - } - else if (i == 1) - { - ref_vec[9][1] = reference[0]; - } - if (i == 2) - { - ref_vec[9][i] = -ref_vec[9][i]; + ref_vec[9][i] = x[0][0]; } - if (abs(ref_vec[9][i]) > 10000) + if (i == 1) { - ref_vec[9][i] = x[2 * i][0]; - if (i == 0) - { - ref_vec[9][i] = x[0][0]; - } - if (i == 1) - { - ref_vec[9][i] = x[2][0]; - } + ref_vec[9][i] = x[2][0]; } } - - if (i > 0) - { - ref_vec[9][i] = -ref_vec[9][i]; - } - for (int j = 0; j < 10; j++) - { - ref_sum[i] += ref_vec[j][i]; - } - ref_sum[i] = ref_sum[i] /= 10;//ref_vec[9][i];// } - - x_des = ref_sum[0]; - if (x_des > 10000) + + if (i > 0) { - x_des = x[0][0]; + ref_vec[9][i] = -ref_vec[9][i]; } - - y_des = ref_sum[1]; - if (y_des > 10000) + for (int j = 0; j < 10; j++) { - y_des = x[2][0]; + ref_sum[i] += ref_vec[j][i]; } + ref_sum[i] = ref_sum[i] /= 10;//ref_vec[9][i];// + } - r[0][0] = -ref_sum[2]; - if (abs(r[0][0]) > 10000) - { - r[0][0] = x[4][0]; - } + x_des = ref_sum[0]; + if (x_des > 10000) + { + x_des = x[0][0]; + } - r[3][0] = reference[8]; - if (r[3][0] > 10000) - { - r[3][0] =-last_yaw; - } + y_des = ref_sum[1]; + if (y_des > 10000) + { + y_des = x[2][0]; + } + + r[0][0] = -ref_sum[2]; + if (abs(r[0][0]) > 10000) + { + r[0][0] = x[4][0]; + } + + r[3][0] = reference[8]; + if (r[3][0] > 10000) + { + r[3][0] =-last_yaw; + } - float lambda_theta = 0.5f; - float lambda_theta_rate = 0.5f; - float lambda_phi = 0.95f; - float lambda_phi_rate = 0.95f; - float lambda_psi = 0.0004f; - float lambda_psi_rate = 0.05f; - float lambda_z = 0.5f; + double lambda_theta = 0.5f; + double lambda_theta_rate = 0.5f; + double lambda_phi = 0.95f; + double lambda_phi_rate = 0.95f; + double lambda_psi = 0.0004f; + double lambda_psi_rate = 0.05f; + double lambda_z = 0.5f; - Angular_velocities_from_PQR(PQR, Angles, Angular_rates); - x[7][0] = Angular_rates[0][0]; - x[9][0] = Angular_rates[1][0]; - x[11][0] = Angular_rates[2][0]; + Angular_velocities_from_PQR(PQR, Angles, Angular_rates); + x[7][0] = Angular_rates[0][0]; + x[9][0] = Angular_rates[1][0]; + x[11][0] = Angular_rates[2][0]; - // -9, -21 are adjustable gains for the z controller - zdotdot = -9 * x[5][0] - 21 * (x[4][0] - r[0][0]); + // -9, -21 are adjustable gains for the z controller + zdotdot = -9 * x[5][0] - 21 * (x[4][0] - r[0][0]); - /**********Iconfig Sliding Mode***********************/ + /**********Iconfig Sliding Mode***********************/ - Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs); - ref_angles[0][0] = refs[0][0]; - ref_angles[1][0] = refs[1][0]; + Sliding_Iconfig(zdotdot, x_des, x[0][0], x[1][0], y_des, x[2][0], x[3][0], x[10][0], refs); + ref_angles[0][0] = refs[0][0]; + ref_angles[1][0] = refs[1][0]; - /* Reference angles to be sent to inner loop */ - r[1][0] = ref_angles[0][0]; //phi ref - if (reference[6] < 10000 && reference[6] != 0.0f) - { - r[1][0] = reference[6]; - } - r[2][0] = ref_angles[1][0]; //theta ref - if (reference[7] < 10000 && reference[7] != 0.0f) - { - r[2][0] = reference[7]; - } + /* Reference angles to be sent to inner loop */ + r[1][0] = ref_angles[0][0]; //phi ref + if (reference[6] < 10000 && reference[6] != 0.0f) + { + r[1][0] = reference[6]; + } + r[2][0] = ref_angles[1][0]; //theta ref + if (reference[7] < 10000 && reference[7] != 0.0f) + { + r[2][0] = reference[7]; + } - /************ Iconfig Control Law *********************/ - // First get integrated uncertainty parameters - delta_z = y[6]; - delta_phi = y[0]; - delta_theta = y[1]; - delta_psi = y[2]; - delta_roll = y[3]; - delta_pitch = y[4]; - delta_yaw = y[5]; + /************ Iconfig Control Law *********************/ + // First get integrated uncertainty parameters + delta_z = y[6]; + delta_phi = y[0]; + delta_theta = y[1]; + delta_psi = y[2]; + delta_roll = y[3]; + delta_pitch = y[4]; + delta_yaw = y[5]; - U1 = (zdotdot + grav)*m + delta_z; + U1 = (zdotdot + grav)*m + delta_z; - y_out[6] = lambda_z*zdotdot; // generate sliding surface in z, .015 is adjustable slope for sliding surface + y_out[6] = lambda_z*zdotdot; // generate sliding surface in z, .015 is adjustable slope for sliding surface - // error in euler angles - S2_phi = x[6][0] - r[1][0]; - S2_theta = x[8][0] - r[2][0]; - S2_psi = -x[10][0] - r[3][0]; - if (S2_psi > PI) - { - S2_psi -= 2*PI; - } - if (S2_psi < -PI) - { - S2_psi += 2 * PI; - } + // error in euler angles + S2_phi = x[6][0] - r[1][0]; + S2_theta = x[8][0] - r[2][0]; + S2_psi = -x[10][0] - r[3][0]; + if (S2_psi > M_PI) + { + S2_psi -= 2 * M_PI; + } + if (S2_psi < -M_PI) + { + S2_psi += 2 * M_PI; + } - // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired - y_out[0] = lambda_phi*S2_phi; - y_out[1] = lambda_theta*S2_theta; - y_out[2] = lambda_psi*S2_psi; - - R_matrix[0][0] = 1; - R_matrix[1][0] = sin(x[6][0] * tan(x[8][0])); - R_matrix[2][0] = cos(x[6][0])*tan(x[8][0]); - R_matrix[1][0] = 0; - R_matrix[1][1] = cos(x[6][0]); - R_matrix[1][2] = -1 * sin(x[6][0]); - R_matrix[2][0] = 0; - R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]); - R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]); - - inverse_3x3(R_matrix, R_inverse); - rollrate_ref = R_inverse[0][0] * S2_phi*k_phi + R_inverse[0][1] * S2_theta*k_theta + R_inverse[0][2] * S2_psi*k_psi - delta_phi; - pitchrate_ref = R_inverse[1][0] * S2_phi*k_phi + R_inverse[1][1] * S2_theta*k_theta + R_inverse[1][2] * S2_psi*k_psi - delta_theta; - yawrate_ref = R_inverse[2][0] * S2_phi*k_phi + R_inverse[2][1] * S2_theta*k_theta + R_inverse[2][2] * S2_psi*k_psi - delta_psi; + // generate delta_dot which goes to integrator variable, sliding surface for 3 euler angles, can adjust sliding surface slope as desired + y_out[0] = lambda_phi*S2_phi; + y_out[1] = lambda_theta*S2_theta; + y_out[2] = lambda_psi*S2_psi; + + R_matrix[0][0] = 1; + R_matrix[1][0] = sin(x[6][0] * tan(x[8][0])); + R_matrix[2][0] = cos(x[6][0])*tan(x[8][0]); + R_matrix[1][0] = 0; + R_matrix[1][1] = cos(x[6][0]); + R_matrix[1][2] = -1 * sin(x[6][0]); + R_matrix[2][0] = 0; + R_matrix[2][1] = sin(x[6][0]) / cos(x[8][0]); + R_matrix[2][2] = cos(x[6][0]) / cos(x[8][0]); + + inverse_3x3(R_matrix, R_inverse); + rollrate_ref = R_inverse[0][0] * S2_phi*k_phi + R_inverse[0][1] * S2_theta*k_theta + R_inverse[0][2] * S2_psi*k_psi - delta_phi; + pitchrate_ref = R_inverse[1][0] * S2_phi*k_phi + R_inverse[1][1] * S2_theta*k_theta + R_inverse[1][2] * S2_psi*k_psi - delta_theta; + yawrate_ref = R_inverse[2][0] * S2_phi*k_phi + R_inverse[2][1] * S2_theta*k_theta + R_inverse[2][2] * S2_psi*k_psi - delta_psi; - if (reference[9] < 10000 && reference[9] != 0.0f) - { - rollrate_ref = reference[9]; - } - if (reference[10] < 10000 && reference[10] != 0.0f) - { - pitchrate_ref = reference[10]; - } - if (reference[11] < 10000) - { - yawrate_ref = reference[11]; - } + if (reference[9] < 10000 && reference[9] != 0.0f) + { + rollrate_ref = reference[9]; + } + if (reference[10] < 10000 && reference[10] != 0.0f) + { + pitchrate_ref = reference[10]; + } + if (reference[11] < 10000) + { + yawrate_ref = reference[11]; + } - PQR_generation(x, PQR); - S3_P = PQR[0][0] - rollrate_ref; - S3_Q = PQR[1][0] - pitchrate_ref; - S3_R = PQR[2][0] - yawrate_ref; + PQR_generation(x, PQR); + S3_P = PQR[0][0] - rollrate_ref; + S3_Q = PQR[1][0] - pitchrate_ref; + S3_R = PQR[2][0] - yawrate_ref; - // Sliding surface for the body frame angular rates to be integrated - y_out[3] = lambda_phi_rate*S3_P; - y_out[4] = lambda_theta_rate*S3_Q; - y_out[5] = lambda_psi_rate*S3_R; + // Sliding surface for the body frame angular rates to be integrated + y_out[3] = lambda_phi_rate*S3_P; + y_out[4] = lambda_theta_rate*S3_Q; + y_out[5] = lambda_psi_rate*S3_R; - // Calculate controls for roll, pitch, and yaw - U2 = k_roll*S3_P*Ix + (Iz - Iy)*PQR[1][0] * PQR[2][0] - delta_roll; - U3 = k_pitch*S3_Q*Iy + (Ix - Iz)*PQR[0][0] * PQR[2][0] - delta_pitch; - U4 = k_yaw*S3_R*Iz + (Iy - Ix)*PQR[0][0] * PQR[1][0] - delta_yaw; + // Calculate controls for roll, pitch, and yaw + U2 = k_roll*S3_P*Ix + (Iz - Iy)*PQR[1][0] * PQR[2][0] - delta_roll; + U3 = k_pitch*S3_Q*Iy + (Ix - Iz)*PQR[0][0] * PQR[2][0] - delta_pitch; + U4 = k_yaw*S3_R*Iz + (Iy - Ix)*PQR[0][0] * PQR[1][0] - delta_yaw; - // Rescale such that the outputs normalize from -1,1 + // Rescale such that the outputs normalize from -1,1 - U1 = U1 / 80;//sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal + U1 = U1 / 80;//sqrt(abs(U1)) / 6.20; // I used sqrt to try and allow for smoother signal - U2 = U2 / 80; + U2 = U2 / 80; - U3 = U3 / 80; + U3 = U3 / 80; - U4 = U4 / 80; + U4 = U4 / 80; - // Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2 - if (U1 > 1) - { - U1 = 1; - } - else if (U1 < 0.35) - { - U1 = 0.35; - } - if (U2 > U2_sat) - { - U2 = U2_sat; - } - else if (U2 < -U2_sat) - { - U2 = -U2_sat; - } - if (U3 > U3_sat) - { - U3 = U3_sat; - } - else if (U3 < -U3_sat) - { - U3 = -U3_sat; - } - if (U4 > U4_sat) - { - U4 = U4_sat; - } - else if (U4 < -U4_sat) - { - U4 = -U4_sat; - } + // Saturations: U1->.35,1 : U2,U3,U4 -> -.2,.2 + if (U1 > 1) + { + U1 = 1; + } + else if (U1 < 0.35) + { + U1 = 0.35; + } + if (U2 > U2_sat) + { + U2 = U2_sat; + } + else if (U2 < -U2_sat) + { + U2 = -U2_sat; + } + if (U3 > U3_sat) + { + U3 = U3_sat; + } + else if (U3 < -U3_sat) + { + U3 = -U3_sat; + } + if (U4 > U4_sat) + { + U4 = U4_sat; + } + else if (U4 < -U4_sat) + { + U4 = -U4_sat; + } - remapU(U1, U2, U3, U4); //remap to axis4r + remapU(U1, U2, U3, U4); //remap to axis4r - } // SlidingModeModel */ + } // SlidingModeModel */ - }; +}; } From 03e977f14a999519f36e73ccd233e2daebaa4d04 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 29 Mar 2018 16:57:43 -0700 Subject: [PATCH 039/121] fixed Clang warnings --- AirLib/include/common/common_utils/Utils.hpp | 2 +- .../firmware/AdaptiveController.hpp | 162 +++++++++--------- 2 files changed, 82 insertions(+), 82 deletions(-) diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index 86e4da0862..38c56496b7 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -472,7 +472,7 @@ class Utils { static std::time_t to_time_t(const std::string& str, bool is_dst = false, const std::string& format = "%Y-%m-%d %H:%M:%S") { - std::tm t = {0}; + std::tm t; t.tm_isdst = is_dst ? 1 : 0; std::istringstream ss(str); ss >> std::get_time(&t, format.c_str()); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index 53881b593f..f602549130 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -102,7 +102,7 @@ class AdaptiveController : public IController { double x_0[12]; GoalMode last_mode_; //double error[3] = { 0 }; - double ref_vec[10][3] = { 0 }; + double ref_vec[10][3] = {{ 0 }}; double ref_sum[3] = { 0 }; double velocity_integrator[3] = { 0 }; static constexpr int array_length = 7; @@ -180,7 +180,7 @@ class AdaptiveController : public IController { void update_goals() { - const auto& mode = goal_->getGoalMode(); + const auto& mode = goal_->getGoalMode(); const auto& value = goal_->getGoalValue(); for (int i = 0; i < 12; i++) @@ -189,87 +189,87 @@ class AdaptiveController : public IController { } int count1 = 0, count2 = 0, count3 = 0, count4 = 0; - for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) - { + for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) + { switch (mode[axis]) { - case GoalModeType::AngleRate: - - switch (count1) - { - case 0: - reference[11] = value[axis]; - break; - case 1: - reference[9] = value[axis]; - break; - case 2: - reference[10] = value[axis]; - break; - } - count1++; - break; - case GoalModeType::AngleLevel: - - switch (count2) - { - case 0: - if (axis == 2) - { - reference[8] = value[axis]; - } - else - { - reference[6] = value[axis]; - } - break; - case 1: - reference[7] = -value[axis]; - break; - case 2: - reference[8] = value[axis]; - break; - } - count2++; - break; - case GoalModeType::VelocityWorld: - - switch (count3) - { - case 0: - reference[3] = value[axis]; - break; - case 1: - reference[4] = value[axis]; - break; - case 2: - reference[5] = value[axis]; - break; - } - count3++; - break; - case GoalModeType::PositionWorld: - - switch (count4) - { - case 0: - if (axis != 0) - { - reference[2] = value[axis]; - } - else - { - reference[0] = value[axis]; - } - break; - case 1: - reference[1] = value[axis]; - break; - case 2: - reference[2] = value[axis]; - break; - } - count4++; + case GoalModeType::AngleRate: + + switch (count1) + { + case 0: + reference[11] = value[axis]; + break; + case 1: + reference[9] = value[axis]; + break; + case 2: + reference[10] = value[axis]; + break; + } + count1++; + break; + case GoalModeType::AngleLevel: + + switch (count2) + { + case 0: + if (axis == 2) + { + reference[8] = value[axis]; + } + else + { + reference[6] = value[axis]; + } + break; + case 1: + reference[7] = -value[axis]; + break; + case 2: + reference[8] = value[axis]; + break; + } + count2++; + break; + case GoalModeType::VelocityWorld: + + switch (count3) + { + case 0: + reference[3] = value[axis]; + break; + case 1: + reference[4] = value[axis]; + break; + case 2: + reference[5] = value[axis]; + break; + } + count3++; + break; + case GoalModeType::PositionWorld: + + switch (count4) + { + case 0: + if (axis != 0) + { + reference[2] = value[axis]; + } + else + { + reference[0] = value[axis]; + } + break; + case 1: + reference[1] = value[axis]; + break; + case 2: + reference[2] = value[axis]; + break; + } + count4++; break; default: From 98dd2836a73e94bb7f540edcfb12f005c9bdd9a1 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 29 Mar 2018 17:42:43 -0700 Subject: [PATCH 040/121] minor changes for https://github.com/Microsoft/AirSim/pull/908 --- MavLinkCom/MavLinkTest/Commands.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MavLinkCom/MavLinkTest/Commands.cpp b/MavLinkCom/MavLinkTest/Commands.cpp index 5b4cfb908b..8597060848 100644 --- a/MavLinkCom/MavLinkTest/Commands.cpp +++ b/MavLinkCom/MavLinkTest/Commands.cpp @@ -1607,11 +1607,11 @@ void GotoCommand::HandleMessage(const MavLinkMessage& message) channel->moveToLocalPosition(tx, ty, tz, is_yaw, static_cast(theading * M_PI / 180)); if (this->hasLocalPosition) { - if (!targetReached && fabsf(x - tx) < nearDelta && fabsf(y - ty) < nearDelta) + if (!targetReached && std::abs(x - tx) < nearDelta && std::abs(y - ty) < nearDelta) { targetReached = true; } - if (targetReached && !settled && (fabs(this->vx) + fabsf(this->vy) + fabsf(this->vz) < almostStationery)) { + if (targetReached && !settled && (fabs(this->vx) + std::abs(this->vy) + std::abs(this->vz) < almostStationery)) { settled = true; // ok, now we can safely switch to loiter. TargetReached(); @@ -2081,7 +2081,7 @@ void SquareCommand::UpdateTarget() float dy = ty - y; float dist = sqrtf((dx*dx) + (dy*dy)); - if (fabsf(dx) < near && fabsf(dy) < near) + if (std::abs(dx) < near && std::abs(dy) < near) { leg_++; if (leg_ == 4) leg_ = 0; From 4c05663f01eb4ac81b1c808d0b4d8d2558cafb5b Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 29 Mar 2018 17:54:18 -0700 Subject: [PATCH 041/121] fix clang warning on Linux --- Unreal/Plugins/AirSim/Source/NedTransform.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 51205abc41..4900aaa3a7 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -28,7 +28,7 @@ class NedTransform Vector3r toVector3r(const FVector& vec, float scale, bool convert_to_ned) const; private: - FVector offset_; - float world_to_meters_; bool is_initialized_; + float world_to_meters_; + FVector offset_; }; From 95853ad9f18b6d1d45103ec60d3bd534ac95e57b Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 29 Mar 2018 23:51:41 -0700 Subject: [PATCH 042/121] added gimble stabilization --- AirLib/include/common/AirSimSettings.hpp | 18 +++++++++++ Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 35 ++++++++++++++++++++++ Unreal/Plugins/AirSim/Source/PIPCamera.h | 3 ++ docs/settings.md | 10 ++++++- 4 files changed, 65 insertions(+), 1 deletion(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index bb59081890..b613a296a0 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -73,6 +73,14 @@ struct AirSimSettings { float roll = 0.0f; }; + struct GimbleSetting { + float stabilization = 0; + //bool is_world_frame = false; + float pitch = Utils::nan(); + float roll = Utils::nan(); + float yaw = Utils::nan(); + }; + struct CaptureSetting { //below settinsg are obtained by using Unreal console command (press ~): // ShowFlag.VisualizeHDR 1. @@ -99,6 +107,7 @@ struct AirSimSettings { int projection_mode = 0; // ECameraProjectionMode::Perspective float ortho_width = Utils::nan(); + GimbleSetting gimble; }; struct NoiseSetting { @@ -518,6 +527,15 @@ struct AirSimSettings { throw std::invalid_argument(std::string("CaptureSettings projection_mode has invalid value in settings ") + projection_mode); capture_setting.ortho_width = settings.getFloat("OrthoWidth", capture_setting.ortho_width); + + Settings json_parent; + if (settings.getChild("Gimble", json_parent)) { + //capture_setting.gimble.is_world_frame = json_parent.getBool("IsWorldFrame", false); + capture_setting.gimble.stabilization = json_parent.getFloat("Stabilization", false); + capture_setting.gimble.pitch = json_parent.getFloat("Pitch", Utils::nan()); + capture_setting.gimble.roll = json_parent.getFloat("Roll", Utils::nan()); + capture_setting.gimble.yaw = json_parent.getFloat("Yaw", Utils::nan()); + } } void loadSubWindowsSettings(const Settings& settings) diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 861344010f..244c3c2042 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -19,6 +19,8 @@ APIPCamera::APIPCamera() else UAirBlueprintLib::LogMessageString("Cannot create noise material for the PIPCamera", "", LogDebugLevel::Failure); + + PrimaryActorTick.bCanEverTick = true; } void APIPCamera::PostInitializeComponents() @@ -68,6 +70,29 @@ void APIPCamera::BeginPlay() render_targets_[image_type] = NewObject(); } + + gimble_stabilization_ = 0; + gimbled_rotator_ = this->GetActorRotation(); + this->SetActorTickEnabled(false); +} + + +void APIPCamera::Tick(float DeltaTime) +{ + if (gimble_stabilization_ > 0) { + FRotator rotator = this->GetActorRotation(); + if (!std::isnan(gimbled_rotator_.Pitch)) + rotator.Pitch = gimbled_rotator_.Pitch * gimble_stabilization_ + + rotator.Pitch * (1 - gimble_stabilization_); + if (!std::isnan(gimbled_rotator_.Roll)) + rotator.Roll = gimbled_rotator_.Roll * gimble_stabilization_ + + rotator.Roll * (1 - gimble_stabilization_); + if (!std::isnan(gimbled_rotator_.Yaw)) + rotator.Yaw = gimbled_rotator_.Yaw * gimble_stabilization_ + + rotator.Yaw * (1 - gimble_stabilization_); + + this->SetActorRotation(rotator); + } } void APIPCamera::EndPlay(const EEndPlayReason::Type EndPlayReason) @@ -134,6 +159,16 @@ void APIPCamera::setImageTypeSettings(int image_type, const APIPCamera::CaptureS updateCameraSetting(camera_, capture_setting, ned_transform_); setNoiseMaterial(image_type, camera_, camera_->PostProcessSettings, noise_setting); + + gimble_stabilization_ = Utils::clip(capture_setting.gimble.stabilization, 0.0f, 1.0f); + if (gimble_stabilization_ > 0) { + this->SetActorTickEnabled(true); + gimbled_rotator_.Pitch = capture_setting.gimble.pitch; + gimbled_rotator_.Roll = capture_setting.gimble.roll; + gimbled_rotator_.Yaw = capture_setting.gimble.yaw; + } + else + this->SetActorTickEnabled(false); } } diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 9e91e8e48c..8b24a1d36e 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -27,6 +27,7 @@ class AIRSIM_API APIPCamera : public ACameraActor virtual void PostInitializeComponents() override; virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; void showToScreen(); @@ -58,6 +59,8 @@ class AIRSIM_API APIPCamera : public ACameraActor std::vector camera_type_enabled_; NedTransform ned_transform_; + FRotator gimbled_rotator_; + float gimble_stabilization_; private: typedef common_utils::Utils Utils; diff --git a/docs/settings.md b/docs/settings.md index 6ce37adb2c..b9ef700e22 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -58,7 +58,13 @@ Below are complete list of settings available along with their default values. I "MotionBlurAmount": 0, "TargetGamma": 1.0, "ProjectionMode": "", - "OrthoWidth": 5.12 + "OrthoWidth": 5.12, + "Gimble": { + "Stabilization": 0, + "Pitch": NaN, + "Roll": NaN, + "Yaw": NaN + } } ], "OriginGeopoint": { @@ -150,6 +156,8 @@ For explanation of other settings, please see [this article](https://docs.unreal The `ImageType` element determines which image type the settings applies to. The valid values are described in [ImageType section](image_apis.md#available-imagetype). In addition, we also support special value `ImageType: -1` to apply the settings to external camera (i.e. what you are looking at on the screen). +The `Gimble` element allows to freeze camera orientation for pitch, roll and/or yaw. This setting is ignored unless `ImageType` is -1. The `Stabilization` is defaulted to 0 meaning no gimble i.e. camera orientation changes with body orientation on all axis. The value of 1 means full stabilization. The value between 0 to 1 acts as a weight for fixed angles specified (in degrees, in world-frame) in `Pitch`, `Roll` and `Yaw` elements and orientation of the vehicle body. When any of the angles is ommitted from json or set to NaN, that angle is not stabilized (i.e. it moves along with vehicle body). + Note that `CaptureSettings` element is json array so you can add settings for multiple image types easily. ## Changing Flight Controller From 036186ab2f9b54d07187462cad599efb72c7b35b Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 30 Mar 2018 00:00:10 -0700 Subject: [PATCH 043/121] doc update for gimble stabilization --- docs/image_apis.md | 2 ++ docs/settings.md | 1 + 2 files changed, 3 insertions(+) diff --git a/docs/image_apis.md b/docs/image_apis.md index 7b68b583b7..a30b38dbc6 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -161,6 +161,8 @@ To change orientation of individial camera, you can use `setCameraOrientation` A client.setCameraOrientation(0, AirSimClientBase.toQuaternion(0.261799, 0, 0)); #radians ``` +You can set stabilization for pitch, roll or yaw for any camera [using settings](settings.md#gimble). + Please see [example usage](https://github.com/Microsoft/AirSim/blob/master/PythonClient/cv_mode.py). ## Changing Resolution and Camera Parameters diff --git a/docs/settings.md b/docs/settings.md index b9ef700e22..487022e922 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -156,6 +156,7 @@ For explanation of other settings, please see [this article](https://docs.unreal The `ImageType` element determines which image type the settings applies to. The valid values are described in [ImageType section](image_apis.md#available-imagetype). In addition, we also support special value `ImageType: -1` to apply the settings to external camera (i.e. what you are looking at on the screen). +### Gimble The `Gimble` element allows to freeze camera orientation for pitch, roll and/or yaw. This setting is ignored unless `ImageType` is -1. The `Stabilization` is defaulted to 0 meaning no gimble i.e. camera orientation changes with body orientation on all axis. The value of 1 means full stabilization. The value between 0 to 1 acts as a weight for fixed angles specified (in degrees, in world-frame) in `Pitch`, `Roll` and `Yaw` elements and orientation of the vehicle body. When any of the angles is ommitted from json or set to NaN, that angle is not stabilized (i.e. it moves along with vehicle body). Note that `CaptureSettings` element is json array so you can add settings for multiple image types easily. From 1c7b250cbbc193f5b323529268c3f0527b293b5d Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 30 Mar 2018 03:13:15 -0700 Subject: [PATCH 044/121] Prep for easily changeable car mesh, packaging error fixes --- AirLib/include/common/AirSimSettings.hpp | 9 +++++++ .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 2 ++ Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 22 +++++----------- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 26 +++++++++++++++++++ .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 8 +----- .../AirSim/Source/VehiclePawnWrapper.cpp | 11 ++++++++ .../AirSim/Source/VehiclePawnWrapper.h | 3 +++ 7 files changed, 59 insertions(+), 22 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index b613a296a0..c69618cd3d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -48,6 +48,14 @@ struct AirSimSettings { } }; + struct CarMeshPaths { + std::string skeletal = "/AirSim/VehicleAdv/Vehicle/Vehicle_SkelMesh.Vehicle_SkelMesh"; + std::string bp = "/AirSim/VehicleAdv/Vehicle/VehicleAnimationBlueprint"; + std::string slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery"; + std::string non_slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery"; + + }; + struct VehicleSettings { std::string vehicle_name, firmware_name; int server_port; @@ -188,6 +196,7 @@ struct AirSimSettings { bool engine_sound; bool log_messages_visible; HomeGeoPoint origin_geopoint; + CarMeshPaths car_mesh_paths; public: //methods static AirSimSettings& singleton() diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index e2c582430c..692792c08c 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -4,6 +4,7 @@ #include "AirBlueprintLib.h" #include "GameFramework/WorldSettings.h" #include "Components/SceneCaptureComponent2D.h" +#include "Components/SkinnedMeshComponent.h" #include "Kismet/GameplayStatics.h" #include "GameFramework/RotatingMovementComponent.h" #include @@ -17,6 +18,7 @@ #include "Kismet/KismetStringLibrary.h" #include "MessageDialog.h" #include "Engine/LocalPlayer.h" +#include "Engine/SkeletalMesh.h" #include "Slate/SceneViewport.h" #include "Engine/Engine.h" diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index f25de2b2dd..84d3424f32 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -1,12 +1,10 @@ #include "CarPawn.h" #include "CarWheelFront.h" #include "CarWheelRear.h" -#include "Components/SkeletalMeshComponent.h" #include "common/common_utils/Utils.hpp" #include "Components/TextRenderComponent.h" #include "Components/AudioComponent.h" #include "Sound/SoundCue.h" -#include "PhysicalMaterials/PhysicalMaterial.h" #include "WheeledVehicleMovementComponent4W.h" #include "Engine/SkeletalMesh.h" #include "GameFramework/Controller.h" @@ -14,7 +12,8 @@ #include "common/ClockFactory.hpp" #include "PIPCamera.h" #include -#include "UObject/ConstructorHelpers.h" + + // Needed for VR Headset #if HMD_MODULE_INCLUDED @@ -33,20 +32,13 @@ ACarPawn::ACarPawn() this->AutoPossessPlayer = EAutoReceiveInput::Player0; //this->AutoReceiveInput = EAutoReceiveInput::Player0; - // Car mesh - static ConstructorHelpers::FObjectFinder CarMesh(TEXT("/AirSim/VehicleAdv/Vehicle/Vehicle_SkelMesh.Vehicle_SkelMesh")); - GetMesh()->SetSkeletalMesh(CarMesh.Object); + static MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); - static ConstructorHelpers::FClassFinder AnimBPClass(TEXT("/AirSim/VehicleAdv/Vehicle/VehicleAnimationBlueprint")); + GetMesh()->SetSkeletalMesh(helpers.skeleton); GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); - GetMesh()->SetAnimInstanceClass(AnimBPClass.Class); - - // Setup friction materials - static ConstructorHelpers::FObjectFinder SlipperyMat(TEXT("/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery")); - SlipperyMaterial = SlipperyMat.Object; - - static ConstructorHelpers::FObjectFinder NonSlipperyMat(TEXT("/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery")); - NonSlipperyMaterial = NonSlipperyMat.Object; + GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); + SlipperyMaterial = helpers.slippery_mat; + NonSlipperyMaterial = helpers.non_slippery_mat; static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 06f9001637..22b9c6001e 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -6,6 +6,10 @@ #include "physics/Kinematics.hpp" #include "CarPawnApi.h" #include "SimJoyStick/SimJoyStick.h" +#include "UObject/ConstructorHelpers.h" +#include "Components/SkeletalMeshComponent.h" +#include "PhysicalMaterials/PhysicalMaterial.h" +#include "common/AirSimSettings.hpp" #include "CarPawn.generated.h" class UPhysicalMaterial; @@ -151,6 +155,28 @@ class ACarPawn : public AWheeledVehicle FORCEINLINE UAudioComponent* GetEngineSoundComponent() const { return EngineSoundComponent; } private: + typedef msr::airlib::AirSimSettings AirSimSettings; + + struct MeshContructionHelpers { + USkeletalMesh* skeleton; + UBlueprint* bp; + UPhysicalMaterial* slippery_mat; + UPhysicalMaterial* non_slippery_mat; + + MeshContructionHelpers(const msr::airlib::AirSimSettings::CarMeshPaths& paths) + { + skeleton = Cast(StaticLoadObject(UObject::StaticClass(), + nullptr, * (FString(paths.skeletal.c_str())))); + bp = Cast(StaticLoadObject(UObject::StaticClass(), + nullptr, * (FString(paths.bp.c_str())))); + slippery_mat = Cast(StaticLoadObject(UObject::StaticClass(), + nullptr, * (FString(paths.slippery_mat.c_str())))); + non_slippery_mat = Cast(StaticLoadObject(UObject::StaticClass(), + nullptr, * (FString(paths.non_slippery_mat.c_str())))); + } + }; + + UClass* pip_camera_class_; std::unique_ptr rpclib_server_; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 4792739589..3e0fcbb7c1 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -128,14 +128,8 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) int ASimModeCar::getRemoteControlID(const VehiclePawnWrapper& pawn) { - typedef msr::airlib::AirSimSettings AirSimSettings; - - //find out which RC we should use - AirSimSettings::VehicleSettings vehicle_settings = - AirSimSettings::singleton().getVehicleSettings(fpv_vehicle_pawn_wrapper_->getVehicleConfigName()); - msr::airlib::Settings settings; - vehicle_settings.getRawSettings(settings); + fpv_vehicle_pawn_wrapper_->getRawVehicleSettings(settings); msr::airlib::Settings rc_settings; settings.getChild("RC", rc_settings); diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index cdc90a1d2f..d54ef67bae 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -90,6 +90,17 @@ void VehiclePawnWrapper::setKinematics(const msr::airlib::Kinematics::State* kin kinematics_ = kinematics; } +void VehiclePawnWrapper::getRawVehicleSettings(msr::airlib::Settings& settings) const +{ + typedef msr::airlib::AirSimSettings AirSimSettings; + + //find out which RC we should use + AirSimSettings::VehicleSettings vehicle_settings = + AirSimSettings::singleton().getVehicleSettings(this->getVehicleConfigName()); + + vehicle_settings.getRawSettings(settings); +} + std::string VehiclePawnWrapper::getVehicleConfigName() const { return getConfig().vehicle_config_name == "" ? msr::airlib::AirSimSettings::singleton().default_vehicle_config diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index da3c47d804..ab52f97d62 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h @@ -90,6 +90,9 @@ class VehiclePawnWrapper const NedTransform& getNedTransform() const; + void getRawVehicleSettings(msr::airlib::Settings& settings) const; + + protected: UPROPERTY(VisibleAnywhere) UParticleSystem* collision_display_template; From 55e9ecb39cf4fa812d94d6808b715587994d334b Mon Sep 17 00:00:00 2001 From: clovett Date: Mon, 2 Apr 2018 01:37:20 -0700 Subject: [PATCH 045/121] Fix bugs to do with PX4 related to landing. Sim was creating unusual z-acc on landing that confused the PX4 HIL mode landing detector. --- .gitignore | 1 + AirLib/include/physics/FastPhysicsEngine.hpp | 74 +++++++++---------- .../magnetometer/MagnetometerSimple.hpp | 1 - .../vehicles/multirotor/MultiRotorParams.hpp | 2 +- PythonClient/AirSimClient.py | 1 - PythonClient/path.py | 3 +- PythonClient/survey.py | 8 +- 7 files changed, 41 insertions(+), 49 deletions(-) diff --git a/.gitignore b/.gitignore index 410abcc9e8..fa8f9ad4ff 100644 --- a/.gitignore +++ b/.gitignore @@ -53,6 +53,7 @@ obj/ # Visual Studio 2015 cache/options directory .vs/ +.vscode/ # MSTest test Results [Tt]est[Rr]esult*/ diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index fe79895790..9d639bf798 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -84,7 +84,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { const CollisionInfo collision_info = body.getCollisionInfo(); CollisionResponseInfo& collision_response_info = body.getCollisionResponseInfo(); //if collision was already responsed then do not respond to it until we get updated information - if (collision_info.has_collided && collision_response_info.collision_time_stamp != collision_info.time_stamp) { + if (grounded_ || (collision_info.has_collided && collision_response_info.collision_time_stamp != collision_info.time_stamp)) { bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body, current, next, next_wrench, enable_ground_lock_, grounded_); updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response_info); @@ -137,12 +137,26 @@ class FastPhysicsEngine : public PhysicsEngineBase { const Vector3r normal_body = VectorMath::transformToBodyFrame(collision_info.normal, current.pose.orientation); const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance); bool ground_collision = false; - if (is_ground_normal - || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance) - || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance) + const float z_vel = vcur_avg.z(); + const bool is_landing = z_vel > std::abs(vcur_avg.x()) && z_vel > std::abs(vcur_avg.y()); + + double restitution = body.getRestitution(); + double friction = body.getFriction(); + + if (is_ground_normal && is_landing + // So normal_body is the collision normal translated into body coords, why does an x==1 or y==1 + // mean we are coliding with the ground??? + // || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance) + // || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance) ) { - //think of collision occured along the surface, not at point - r = Vector3r::Zero(); + // looks like we are coliding with the ground. We don't want the ground to be so bouncy + // so we reduce the coefficient of restitution. 0 means no bounce. + // TODO: it would be better if we did this based on the material we are landing on. + // e.g. grass should be inelastic, but a hard surface like the road should be more bouncy. + restitution = 0; + // crank up friction with the ground so it doesn't try and slide across the ground + // again, this should depend on the type of surface we are landing on. + friction = 1; //we have collided with ground straight on, we will fix orientation later ground_collision = is_ground_normal; @@ -165,7 +179,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { (body.getInertiaInv() * r.cross(normal_body)) .cross(r) .dot(normal_body); - const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + body.getRestitution()) / impulse_mag_denom; + const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + restitution) / impulse_mag_denom; next.twist.linear = vcur_avg + collision_info.normal * (impulse_mag / body.getMass()); next.twist.angular = angular_avg + r.cross(normal_body) * impulse_mag; @@ -178,7 +192,7 @@ class FastPhysicsEngine : public PhysicsEngineBase { (body.getInertiaInv() * r.cross(contact_tang_unit_body)) .cross(r) .dot(contact_tang_unit_body); - const real_T friction_mag = -contact_tang_body.norm() * body.getFriction() / friction_mag_denom; + const real_T friction_mag = -contact_tang_body.norm() * friction / friction_mag_denom; const Vector3r contact_tang_unit = VectorMath::transformToWorldFrame(contact_tang_unit_body, current.pose.orientation); next.twist.linear += contact_tang_unit * friction_mag; @@ -188,9 +202,9 @@ class FastPhysicsEngine : public PhysicsEngineBase { next.twist.angular *= 0.9f; //there is no acceleration during collision response - next.accelerations.linear = Vector3r::Zero(); - next.accelerations.angular = Vector3r::Zero(); - + //next.accelerations.linear = Vector3r::Zero(); + //next.accelerations.angular = Vector3r::Zero(); + next.pose = current.pose; if (enable_ground_lock && ground_collision) { float pitch, roll, yaw; @@ -205,6 +219,11 @@ class FastPhysicsEngine : public PhysicsEngineBase { next.twist.linear = Vector3r::Zero(); next.pose.position = collision_info.position; grounded = true; + + // but we do want to "feel" the ground when we hit it (we should see a small z-acc bump) + // equal and opposite our downward velocity. + next.accelerations.linear = -0.5 * body.getMass() * vcur_avg; + //throttledLogOutput("*** Triggering ground lock", 0.1); } else @@ -230,33 +249,6 @@ class FastPhysicsEngine : public PhysicsEngineBase { } } - //bool getNextKinematicsOnGround(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench) - //{ - // /************************* reset state if we have hit the ground ************************/ - // real_T min_z_over_ground = body.getEnvironment().getState().min_z_over_ground; - // grounded_ = 0; - // if (min_z_over_ground <= next.pose.position.z()) { - // grounded_ = 1; - // next.pose.position.z() = min_z_over_ground; - - // real_T z_proj = static_cast(next.twist.linear.z() + next.accelerations.linear.z() * dt); - // if (Utils::isDefinitelyLessThan(0.0f, z_proj)) { - // grounded_ = 2; - // next.twist = Twist::zero(); - // next.accelerations.linear = Vector3r::Zero(); - // next.accelerations.angular = Vector3r::Zero(); - // //reset roll/pitch - px4 seems to have issue with this - // real_T r, p, y; - // VectorMath::toEulerianAngle(current.pose.orientation, p, r, y); - // next.pose.orientation = VectorMath::toQuaternion(0, 0, y); - - // next_wrench = Wrench::zero(); - // } - // } - - // return grounded_ != 0; - //} - static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation, const Vector3r& linear_vel, const Vector3r& angular_vel_body) { @@ -327,9 +319,10 @@ class FastPhysicsEngine : public PhysicsEngineBase { const Wrench body_wrench = getBodyWrench(body, current.pose.orientation); if (grounded) { - // make it stick to the ground until we see significant body wrench forces. + // make it stick to the ground until we see body wrench force greater than gravity. float normalizedForce = body_wrench.force.squaredNorm(); - if (normalizedForce > kSurfaceTension) + float normalizedGravity = body.getEnvironment().getState().gravity.squaredNorm(); + if (normalizedForce >= normalizedGravity) { //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1); grounded = false; @@ -446,7 +439,6 @@ class FastPhysicsEngine : public PhysicsEngineBase { static constexpr float kAxisTolerance = 0.25f; static constexpr float kRestingVelocityMax = 0.1f; static constexpr float kDragMinVelocity = 0.1f; - static constexpr float kSurfaceTension = 1.0f; std::stringstream debug_string_; bool grounded_ = false; diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 51ef587791..5cd3f64e26 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -86,7 +86,6 @@ class MagnetometerSimple : public MagnetometerBase { if (params_.dynamic_reference_source) updateReference(ground_truth); - // Calculate the magnetic field noise. // Calculate the magnetic field noise. output.magnetic_field_body = VectorMath::transformToBodyFrame(magnetic_field_true_, ground_truth.kinematics->pose.orientation, true) * params_.scale_factor diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index 11ace75ce3..4f87b219de 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -51,7 +51,7 @@ class MultiRotorParams { //angular coefficient is usually 10X smaller than linear, however we should replace this with exact number //http://physics.stackexchange.com/q/304742/14061 real_T angular_drag_coefficient = linear_drag_coefficient; - real_T restitution = 0.15f; + real_T restitution = 0.15f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. real_T friction = 0.7f; EnabledSensors enabled_sensors; RotorParams rotor_params; diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 0072811df0..78652e2fed 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -217,7 +217,6 @@ def reset(self): self.client.call('reset') def confirmConnection(self): - print('Waiting for connection: ', end='') home = self.getHomeGeoPoint() while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or math.isnan(home.latitude) or math.isnan(home.longitude) or math.isnan(home.altitude)): diff --git a/PythonClient/path.py b/PythonClient/path.py index 29050bb629..c9d7696d9d 100644 --- a/PythonClient/path.py +++ b/PythonClient/path.py @@ -17,8 +17,9 @@ # this method is async and we are not waiting for the result since we are passing max_wait_seconds=0. result = client.moveOnPath([Vector3r(0,-253,z),Vector3r(125,-253,z),Vector3r(125,0,z),Vector3r(0,0,z),Vector3r(0,0,-20)], - 15, 65, + 12, 120, DrivetrainType.ForwardOnly, YawMode(False,0), 20, 1) +client.moveToPosition(0,0,z,1) client.land() client.armDisarm(False) client.enableApiControl(False) diff --git a/PythonClient/survey.py b/PythonClient/survey.py index 094b056e69..c3dfa68c8e 100644 --- a/PythonClient/survey.py +++ b/PythonClient/survey.py @@ -80,10 +80,10 @@ def start(self): args = sys.argv args.pop(0) arg_parser = argparse.ArgumentParser("Usage: survey boxsize stripewidth altitude") - arg_parser.add_argument("--size", type=float, help="size of the box to survey") - arg_parser.add_argument("--stripewidth", type=float, help="stripe width of survey (in meters)") - arg_parser.add_argument("--altitude", type=float, help="altitude of survey (in positive meters)") - arg_parser.add_argument("--speed", type=float, help="speed of survey (in meters/second)") + arg_parser.add_argument("--size", type=float, help="size of the box to survey", default=50) + arg_parser.add_argument("--stripewidth", type=float, help="stripe width of survey (in meters)", default=10) + arg_parser.add_argument("--altitude", type=float, help="altitude of survey (in positive meters)", default=30) + arg_parser.add_argument("--speed", type=float, help="speed of survey (in meters/second)", default=5) args = arg_parser.parse_args(args) nav = SurveyNavigator(args) nav.start() From b181290fc0dab849861a471ec90c8bf8c38c886b Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Mon, 2 Apr 2018 14:40:16 -0700 Subject: [PATCH 046/121] fix compiler warnings and make drone more bouncy when it hits a tree. --- AirLib/include/physics/FastPhysicsEngine.hpp | 4 ++-- AirLib/include/vehicles/multirotor/MultiRotorParams.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 9d639bf798..c5a937fec3 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -140,8 +140,8 @@ class FastPhysicsEngine : public PhysicsEngineBase { const float z_vel = vcur_avg.z(); const bool is_landing = z_vel > std::abs(vcur_avg.x()) && z_vel > std::abs(vcur_avg.y()); - double restitution = body.getRestitution(); - double friction = body.getFriction(); + real_T restitution = body.getRestitution(); + real_T friction = body.getFriction(); if (is_ground_normal && is_landing // So normal_body is the collision normal translated into body coords, why does an x==1 or y==1 diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index 4f87b219de..b5a4cdd3f0 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -51,8 +51,8 @@ class MultiRotorParams { //angular coefficient is usually 10X smaller than linear, however we should replace this with exact number //http://physics.stackexchange.com/q/304742/14061 real_T angular_drag_coefficient = linear_drag_coefficient; - real_T restitution = 0.15f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. - real_T friction = 0.7f; + real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. + real_T friction = 0.5f; EnabledSensors enabled_sensors; RotorParams rotor_params; From 6aa642483dd3e47664bf6057be5e7d30f3bacda6 Mon Sep 17 00:00:00 2001 From: jfkeller Date: Tue, 3 Apr 2018 14:06:35 -0400 Subject: [PATCH 047/121] Removed a cast to float to fix a bug where the time delta was being calculated incorrectly. --- .../firmwares/simple_flight/firmware/PidController.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp index 96a9dfd24d..af9d2161c3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp @@ -91,7 +91,7 @@ class PidController : public IUpdatable { const T error = goal_ - measured_; float dt = clock_ == nullptr ? 1 : - (static_cast(clock_->millis()) - last_time_) + (clock_->millis() - last_time_) * config_.time_scale; float pterm = error * config_.kp; @@ -135,4 +135,4 @@ class PidController : public IUpdatable { }; -} //namespace \ No newline at end of file +} //namespace From fbd60742ab16ca34e5daaea5403f3d9c031b1a5a Mon Sep 17 00:00:00 2001 From: clovett Date: Wed, 4 Apr 2018 00:21:21 -0700 Subject: [PATCH 048/121] publish new version. --- LogViewer/LogViewer/LogViewer.csproj | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj index 593b3f95a5..5622abfc25 100644 --- a/LogViewer/LogViewer/LogViewer.csproj +++ b/LogViewer/LogViewer/LogViewer.csproj @@ -16,7 +16,7 @@ true true - ftp://www.lovettsoftware.com/LovettSoftware/Downloads/Px4LogViewer/ + ftp://lovettsoftware.com/lovettsoftware.com/Downloads/Px4LogViewer/ true Web true @@ -26,14 +26,14 @@ false false true - http://www.lovettsoftware.com/Downloads/Px4LogViewer/ + http://lovettsoftware.com/Downloads/Px4LogViewer/ http://www.lovettsoftware.com PX4 Log Viewer Lovett Software PX4 Log Viewer true publish.htm - 45 + 46 1.0.0.%2a false true @@ -68,7 +68,7 @@ $(MyKeyFile) - 235CA38984188F94FC230E1DAED8482975D38CFF + AFFF93C9E6AD77B2519D1778A820B789F9F1E31D Graph.ico From c343653d150d0e0a990860f0bccc733699ca1d46 Mon Sep 17 00:00:00 2001 From: Henrik Papp Date: Wed, 4 Apr 2018 17:08:51 +0200 Subject: [PATCH 049/121] Option for front camera view added --- Unreal/Plugins/AirSim/Source/CameraDirector.cpp | 13 +++++++++++++ Unreal/Plugins/AirSim/Source/CameraDirector.h | 8 ++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index 756b87e1ef..458b4c3f95 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -78,6 +78,7 @@ void ACameraDirector::setCameras(APIPCamera* external_camera, VehiclePawnWrapper external_camera_ = external_camera; follow_actor_ = vehicle_pawn_wrapper->getPawn(); fpv_camera_ = vehicle_pawn_wrapper->getCameraCount() > fpv_camera_index_ ? vehicle_pawn_wrapper->getCamera(fpv_camera_index_) : nullptr; + front_camera_ = vehicle_pawn_wrapper->getCameraCount() > front_camera_index_ ? vehicle_pawn_wrapper->getCamera(front_camera_index_) : nullptr; backup_camera_ = backup_camera_index_ >= 0 && vehicle_pawn_wrapper->getCameraCount() > backup_camera_index_ ? vehicle_pawn_wrapper->getCamera(backup_camera_index_) : nullptr; camera_start_location_ = external_camera_->GetActorLocation(); camera_start_rotation_ = external_camera_->GetActorRotation(); @@ -94,6 +95,7 @@ void ACameraDirector::setCameras(APIPCamera* external_camera, VehiclePawnWrapper case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE: inputEventSpringArmChaseView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_BACKUP: inputEventBackupView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY: inputEventNoDisplayView(); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT: inputEventFrontView(); break; default: throw std::out_of_range("Unsupported view mode specified in CameraDirector::initializeForBeginPlay"); } @@ -199,6 +201,7 @@ void ACameraDirector::setupInputBindings() UAirBlueprintLib::BindActionToKey("inputEventSpringArmChaseView", EKeys::Slash, this, &ACameraDirector::inputEventSpringArmChaseView); UAirBlueprintLib::BindActionToKey("inputEventBackupView", EKeys::K, this, &ACameraDirector::inputEventBackupView); UAirBlueprintLib::BindActionToKey("inputEventNoDisplayView", EKeys::Hyphen, this, &ACameraDirector::inputEventNoDisplayView); + UAirBlueprintLib::BindActionToKey("inputEventFrontView", EKeys::I, this, &ACameraDirector::inputEventFrontView); } @@ -212,6 +215,16 @@ void ACameraDirector::inputEventFpvView() fpv_camera_->showToScreen(); } +void ACameraDirector::inputEventFrontView() +{ + setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT); + external_camera_->disableMain(); + if (backup_camera_) + backup_camera_->disableMain(); + if (front_camera_) + front_camera_->showToScreen(); +} + void ACameraDirector::inputEventSpringArmChaseView() { setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE); diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.h b/Unreal/Plugins/AirSim/Source/CameraDirector.h index 51f0857903..960eb13453 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.h +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.h @@ -19,7 +19,8 @@ enum class ECameraDirectorMode : uint8 CAMERA_DIRECTOR_MODE_MANUAL = 4 UMETA(DisplayName = "Manual"), CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE = 5 UMETA(DisplayName = "SpringArmChase"), CAMERA_DIRECTOR_MODE_BACKUP = 6 UMETA(DisplayName = "Backup"), - CAMERA_DIRECTOR_MODE_NODISPLAY = 7 UMETA(DisplayName = "No Display") + CAMERA_DIRECTOR_MODE_NODISPLAY = 7 UMETA(DisplayName = "No Display"), + CAMERA_DIRECTOR_MODE_FRONT = 8 UMETA(DisplayName = "Front") }; UCLASS() @@ -40,6 +41,7 @@ class AIRSIM_API ACameraDirector : public AActor void inputEventSpringArmChaseView(); void inputEventBackupView(); void inputEventNoDisplayView(); + void inputEventFrontView(); public: ACameraDirector(); @@ -81,6 +83,7 @@ class AIRSIM_API ACameraDirector : public AActor APIPCamera* fpv_camera_; APIPCamera* backup_camera_; APIPCamera* external_camera_; + APIPCamera* front_camera_; AActor* follow_actor_; USceneComponent* last_parent_ = nullptr; @@ -96,4 +99,5 @@ class AIRSIM_API ACameraDirector : public AActor bool camera_rotation_lag_enabled_; int fpv_camera_index_; int backup_camera_index_ = 4; -}; + int front_camera_index_ = 0; +}; \ No newline at end of file From bf7783725f80c93055204450c87854a978a4ab40 Mon Sep 17 00:00:00 2001 From: Henrik Papp Date: Wed, 4 Apr 2018 17:20:21 +0200 Subject: [PATCH 050/121] Front ViewMode added --- AirLib/include/common/AirSimSettings.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index c69618cd3d..304980339e 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -370,6 +370,8 @@ struct AirSimSettings { initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP; else if (view_mode_string == "NoDisplay") initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; + else if (view_mode_string == "Front") + initial_view_mode = 8; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; else warning_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); } From 802769a57a62b08d0824c9f6117f63aa973d6fab Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 4 Apr 2018 15:38:37 -0700 Subject: [PATCH 051/121] mintor fixes --- AirLib/include/physics/FastPhysicsEngine.hpp | 7 ++++--- .../LogViewer/Controls/SimpleLineChart.xaml.cs | 4 ++++ LogViewer/LogViewer/MainWindow.xaml.cs | 11 ++--------- .../LogViewer/Utilities/XamlExtensions.cs | 18 ++++++++++++++---- 4 files changed, 24 insertions(+), 16 deletions(-) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index c5a937fec3..406c0a2de7 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -201,9 +201,10 @@ class FastPhysicsEngine : public PhysicsEngineBase { //TODO: implement better rolling friction next.twist.angular *= 0.9f; - //there is no acceleration during collision response - //next.accelerations.linear = Vector3r::Zero(); - //next.accelerations.angular = Vector3r::Zero(); + // there is no acceleration during collision response, this is a hack, but without it the acceleration cancels + // the computed impulse response too much and stops the vehicle from bouncing off the collided object. + next.accelerations.linear = Vector3r::Zero(); + next.accelerations.angular = Vector3r::Zero(); next.pose = current.pose; if (enable_ground_lock && ground_collision) { diff --git a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs index b6932c5d4f..e5d164595f 100644 --- a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs +++ b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml.cs @@ -460,6 +460,10 @@ private void SmoothScroll(DataValue newValue) void UpdateChart() { + if (this.series == null) + { + this.series = new DataSeries() { Name = "", Values = new List() }; + } Canvas.SetLeft(Graph, 0); visibleCount = 0; this.smoothScrollIndex = 0; diff --git a/LogViewer/LogViewer/MainWindow.xaml.cs b/LogViewer/LogViewer/MainWindow.xaml.cs index 2918890fb1..88bd382973 100644 --- a/LogViewer/LogViewer/MainWindow.xaml.cs +++ b/LogViewer/LogViewer/MainWindow.xaml.cs @@ -18,7 +18,6 @@ using System.Windows.Documents; using System.Windows.Input; using System.Windows.Media; -using System.Windows.Media.Animation; using System.Windows.Media.Imaging; using System.Windows.Media.Media3D; using System.Xml.Linq; @@ -1522,13 +1521,7 @@ private void OnClearZoom(object sender, RoutedEventArgs e) private void OnConnectorClick(object sender, MouseButtonEventArgs e) { ConnectionPanel.Start(); - TranslateTransform transform = new TranslateTransform(300, 0); - ConnectionPanel.RenderTransform = transform; - transform.BeginAnimation(TranslateTransform.XProperty, - new DoubleAnimation(0, new Duration(TimeSpan.FromSeconds(0.2))) - { - EasingFunction = new ExponentialEase() { EasingMode = EasingMode.EaseOut } - }); + XamlExtensions.Flyout(ConnectionPanel); } private void OnOpenFileCommand(object sender, ExecutedRoutedEventArgs e) @@ -1680,7 +1673,7 @@ void SendMessage(MAVLink.MAVLINK_MSG_ID id, object mavlinkPayload) private void OnSettings(object sender, RoutedEventArgs e) { - AppSettingsPanel.Visibility = Visibility.Visible; + XamlExtensions.Flyout(AppSettingsPanel); } private void OnMapTest(object sender, RoutedEventArgs e) diff --git a/LogViewer/LogViewer/Utilities/XamlExtensions.cs b/LogViewer/LogViewer/Utilities/XamlExtensions.cs index ee662fe351..92b68d4eb7 100644 --- a/LogViewer/LogViewer/Utilities/XamlExtensions.cs +++ b/LogViewer/LogViewer/Utilities/XamlExtensions.cs @@ -1,19 +1,29 @@ using System; using System.Collections.Generic; using System.IO; -using System.Linq; using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; using System.Windows; -using System.Windows.Controls; using System.Windows.Media; +using System.Windows.Media.Animation; using System.Windows.Media.Imaging; namespace LogViewer.Utilities { public static class XamlExtensions { + public static void Flyout(this FrameworkElement e) + { + e.Visibility = Visibility.Visible; + e.UpdateLayout(); + double width = e.ActualWidth; + TranslateTransform transform = new TranslateTransform(width, 0); + e.RenderTransform = transform; + transform.BeginAnimation(TranslateTransform.XProperty, + new DoubleAnimation(0, new Duration(TimeSpan.FromSeconds(0.2))) + { + EasingFunction = new ExponentialEase() { EasingMode = EasingMode.EaseOut } + }); + } public static T FindAncestorOfType(this DependencyObject d) where T : DependencyObject { From 87fcf321a312404c60d9a3bf9f18cdbab1769be9 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 4 Apr 2018 18:04:20 -0700 Subject: [PATCH 052/121] Fix crash that happens when loading cooked content with missing Car assets by adding more logging information to help user understand what happened, and moving the asset loading to later in the process (non-static). --- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 39 ++++++++++++--- .../Plugins/AirSim/Source/AirBlueprintLib.h | 3 ++ Unreal/Plugins/AirSim/Source/AirSim.cpp | 47 ++++++++++--------- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 24 +++++----- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 14 +++--- 5 files changed, 78 insertions(+), 49 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 692792c08c..1323f7513f 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -29,7 +29,6 @@ Methods -> CamelCase parameters -> camel_case */ - bool UAirBlueprintLib::log_messages_hidden = false; uint32_t UAirBlueprintLib::FlushOnDrawCount = 0; msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method = @@ -150,6 +149,7 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, if (log_messages_hidden) return; + static TMap loggingKeys; static int counter = 1; @@ -161,13 +161,27 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, FColor color; switch (level) { - case LogDebugLevel::Informational: color = FColor(147, 231, 237); break; - case LogDebugLevel::Success: color = FColor(156, 237, 147); break; - case LogDebugLevel::Failure: color = FColor(237, 147, 168); break; - case LogDebugLevel::Unimportant: color = FColor(237, 228, 147); break; + case LogDebugLevel::Informational: + color = FColor(147, 231, 237); + UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); + break; + case LogDebugLevel::Success: + color = FColor(156, 237, 147); + UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); + break; + case LogDebugLevel::Failure: + color = FColor(237, 147, 168); + UE_LOG(LogAirSim, Error, TEXT("%s%s"), *prefix, *suffix); + break; + case LogDebugLevel::Unimportant: + color = FColor(237, 228, 147); + UE_LOG(LogAirSim, Verbose, TEXT("%s%s"), *prefix, *suffix); + break; default: color = FColor::Black; break; } - GEngine->AddOnScreenDebugMessage(key, persist_sec, color, prefix + suffix); + if (GEngine) { + GEngine->AddOnScreenDebugMessage(key, persist_sec, color, prefix + suffix); + } //GEngine->AddOnScreenDebugMessage(key + 10, 60.0f, color, FString::FromInt(key)); } @@ -584,3 +598,16 @@ void UAirBlueprintLib::EnableInput(AActor* actor) { actor->EnableInput(actor->GetWorld()->GetFirstPlayerController()); } + +UObject* UAirBlueprintLib::LoadObject(const std::string& name) +{ + FString str(name.c_str()); + UObject *obj = StaticLoadObject(UObject::StaticClass(), nullptr, *str); + if (obj == nullptr) { + std::string msg = "Failed to load asset - " + name; + FString fmsg(msg.c_str()); + LogMessage(TEXT("Load: "), fmsg, LogDebugLevel::Failure); + throw std::invalid_argument(msg); + } + return obj; +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 08794aa959..9b69da797b 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -13,6 +13,7 @@ #include "Kismet/KismetMathLibrary.h" #include "Components/MeshComponent.h" #include "LandscapeProxy.h" +#include "AirSim.h" #include "common/AirSimSettings.hpp" #include "AirBlueprintLib.generated.h" @@ -105,6 +106,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void resetSimulatePhysics(AActor* actor); static std::vector getPhysicsComponents(AActor* actor); + static UObject* LoadObject(const std::string& name); + private: template static void InitializeObjectStencilID(T* obj, bool ignore_existing = true); diff --git a/Unreal/Plugins/AirSim/Source/AirSim.cpp b/Unreal/Plugins/AirSim/Source/AirSim.cpp index eb8ea3fd94..267af634e3 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.cpp +++ b/Unreal/Plugins/AirSim/Source/AirSim.cpp @@ -1,25 +1,26 @@ // Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#include "AirSim.h" -#include "Misc/Paths.h" - -DEFINE_LOG_CATEGORY(LogAirSim); - -class FAirSim : public IModuleInterface -{ - virtual void StartupModule() override; - virtual void ShutdownModule() override; -}; - -IMPLEMENT_MODULE(FAirSim, AirSim) - -void FAirSim::StartupModule() -{ - //plugin startup -} - -void FAirSim::ShutdownModule() -{ - //plugin shutdown +// Licensed under the MIT License. + +#include "AirSim.h" +#include "Misc/Paths.h" + +DEFINE_LOG_CATEGORY(LogAirSim); + +class FAirSim : public IModuleInterface +{ + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; + +IMPLEMENT_MODULE(FAirSim, AirSim) + +void FAirSim::StartupModule() +{ + //plugin startup + UE_LOG(LogAirSim, Log, TEXT("StartupModule: AirSim plugin")); +} + +void FAirSim::ShutdownModule() +{ + //plugin shutdown } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 84d3424f32..a483028f1d 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -32,17 +32,6 @@ ACarPawn::ACarPawn() this->AutoPossessPlayer = EAutoReceiveInput::Player0; //this->AutoReceiveInput = EAutoReceiveInput::Player0; - static MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); - - GetMesh()->SetSkeletalMesh(helpers.skeleton); - GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); - GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); - SlipperyMaterial = helpers.slippery_mat; - NonSlipperyMaterial = helpers.non_slippery_mat; - - static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); - pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; - UWheeledVehicleMovementComponent4W* Vehicle4W = CastChecked(GetVehicleMovement()); check(Vehicle4W->WheelSetups.Num() == 4); @@ -148,7 +137,7 @@ ACarPawn::ACarPawn() InCarGear->SetVisibility(true); // Setup the audio component and allocate it a sound cue - static ConstructorHelpers::FObjectFinder SoundCue(TEXT("/AirSim/VehicleAdv/Sound/Engine_Loop_Cue.Engine_Loop_Cue")); + ConstructorHelpers::FObjectFinder SoundCue(TEXT("/AirSim/VehicleAdv/Sound/Engine_Loop_Cue.Engine_Loop_Cue")); EngineSoundComponent = CreateDefaultSubobject(TEXT("EngineSound")); EngineSoundComponent->SetSound(SoundCue.Object); EngineSoundComponent->SetupAttachment(GetMesh()); @@ -176,6 +165,17 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se else EngineSoundComponent->Deactivate(); + // load assets + MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); + GetMesh()->SetSkeletalMesh(helpers.skeleton); + GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); + GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); + SlipperyMaterial = helpers.slippery_mat; + NonSlipperyMaterial = helpers.non_slippery_mat; + + ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); + pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + //put camera little bit above vehicle FTransform camera_transform(FVector::ZeroVector); FActorSpawnParameters camera_spawn_params; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 22b9c6001e..61e0b72a22 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -10,6 +10,7 @@ #include "Components/SkeletalMeshComponent.h" #include "PhysicalMaterials/PhysicalMaterial.h" #include "common/AirSimSettings.hpp" +#include "AirBlueprintLib.h" #include "CarPawn.generated.h" class UPhysicalMaterial; @@ -165,15 +166,12 @@ class ACarPawn : public AWheeledVehicle MeshContructionHelpers(const msr::airlib::AirSimSettings::CarMeshPaths& paths) { - skeleton = Cast(StaticLoadObject(UObject::StaticClass(), - nullptr, * (FString(paths.skeletal.c_str())))); - bp = Cast(StaticLoadObject(UObject::StaticClass(), - nullptr, * (FString(paths.bp.c_str())))); - slippery_mat = Cast(StaticLoadObject(UObject::StaticClass(), - nullptr, * (FString(paths.slippery_mat.c_str())))); - non_slippery_mat = Cast(StaticLoadObject(UObject::StaticClass(), - nullptr, * (FString(paths.non_slippery_mat.c_str())))); + skeleton = Cast(UAirBlueprintLib::LoadObject(paths.skeletal)); + bp = Cast(UAirBlueprintLib::LoadObject(paths.bp)); + slippery_mat = Cast(UAirBlueprintLib::LoadObject(paths.slippery_mat)); + non_slippery_mat = Cast(UAirBlueprintLib::LoadObject(paths.non_slippery_mat)); } + }; From 87f07970e2b2a03f4e9d0bb93205a7599e061bbb Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 4 Apr 2018 19:34:10 -0700 Subject: [PATCH 053/121] Fix crash when launching cooked environment created with SUV vehicle. --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 24 +++++++++++--------- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 5 ++++ 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index a483028f1d..83a0275fdc 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -34,6 +34,19 @@ ACarPawn::ACarPawn() UWheeledVehicleMovementComponent4W* Vehicle4W = CastChecked(GetVehicleMovement()); + // load assets + if (UseDefaultMesh) { + static MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); + GetMesh()->SetSkeletalMesh(helpers.skeleton); + GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); + GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); + SlipperyMaterial = helpers.slippery_mat; + NonSlipperyMaterial = helpers.non_slippery_mat; + } + + static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); + pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + check(Vehicle4W->WheelSetups.Num() == 4); // Wheels/Tyres @@ -165,17 +178,6 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se else EngineSoundComponent->Deactivate(); - // load assets - MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); - GetMesh()->SetSkeletalMesh(helpers.skeleton); - GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); - GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); - SlipperyMaterial = helpers.slippery_mat; - NonSlipperyMaterial = helpers.non_slippery_mat; - - ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); - pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; - //put camera little bit above vehicle FTransform camera_transform(FVector::ZeroVector); FActorSpawnParameters camera_spawn_params; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 61e0b72a22..184d6d9e6b 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -65,6 +65,11 @@ class ACarPawn : public AWheeledVehicle UPROPERTY(Category = Display, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) UAudioComponent* EngineSoundComponent; + + /** Whether to load the default meshes */ + UPROPERTY(Category = Display, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) + bool UseDefaultMesh = false; + public: ACarPawn(); From 9d4c844bd6f6e7c0807c70457195078f5e09f0f0 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 5 Apr 2018 21:33:49 -0700 Subject: [PATCH 054/121] Add an orbit script for drones. --- PythonClient/box.py | 2 +- PythonClient/car_monitor.py | 24 +++++ PythonClient/disarm.py | 5 + PythonClient/hello_drone.py | 4 + PythonClient/land.py | 16 ++++ PythonClient/orbit.py | 180 ++++++++++++++++++++++++++++++++++++ PythonClient/path.py | 12 ++- PythonClient/survey.py | 5 +- PythonClient/takeoff.py | 15 +++ 9 files changed, 258 insertions(+), 5 deletions(-) create mode 100644 PythonClient/car_monitor.py create mode 100644 PythonClient/disarm.py create mode 100644 PythonClient/land.py create mode 100644 PythonClient/orbit.py create mode 100644 PythonClient/takeoff.py diff --git a/PythonClient/box.py b/PythonClient/box.py index 605155a194..ce745308ab 100644 --- a/PythonClient/box.py +++ b/PythonClient/box.py @@ -18,7 +18,7 @@ # Fly given velocity vector for 5 seconds duration = 5 speed = 1 -delay = duration * speed; +delay = duration * speed # using DrivetrainType.MaxDegreeOfFreedom means we can control the drone yaw independently # from the direction the drone is flying. I've set values here that make the drone always point inwards diff --git a/PythonClient/car_monitor.py b/PythonClient/car_monitor.py new file mode 100644 index 0000000000..4772b2d739 --- /dev/null +++ b/PythonClient/car_monitor.py @@ -0,0 +1,24 @@ +from AirSimClient import * +import cv2 +import time + +# connect to the AirSim simulator +client = CarClient() +client.confirmConnection() +car_controls = CarControls() + +start = time.time() + +print("Time,Speed,Gear,PX,PY,PZ,OW,OX,OY,OZ") + +# monitor car state while you drive it manually. +while (cv2.waitKey(1) & 0xFF) == 0xFF: + # get state of the car + car_state = client.getCarState() + pos = car_state.kinematics_true.position + orientation = car_state.kinematics_true.orientation + milliseconds = (time.time() - start) * 1000 + print("%s,%d,%d,%f,%f,%f,%f,%f,%f,%f" % \ + (milliseconds, car_state.speed, car_state.gear, pos.x_val, pos.y_val, pos.z_val, + orientation.w_val, orientation.x_val, orientation.y_val, orientation.z_val)) + time.sleep(0.1) diff --git a/PythonClient/disarm.py b/PythonClient/disarm.py new file mode 100644 index 0000000000..a65bdb84d7 --- /dev/null +++ b/PythonClient/disarm.py @@ -0,0 +1,5 @@ +from AirSimClient import * + +client = MultirotorClient() +client.confirmConnection() +client.armDisarm(False) diff --git a/PythonClient/hello_drone.py b/PythonClient/hello_drone.py index be5aea568a..cb82631831 100644 --- a/PythonClient/hello_drone.py +++ b/PythonClient/hello_drone.py @@ -28,6 +28,8 @@ AirSimClientBase.wait_key('Press any key to move vehicle to (-10, 10, -10) at 5 m/s') client.moveToPosition(-10, 10, -10, 5) +client.hover() + state = client.getMultirotorState() print("state: %s" % pprint.pformat(state)) @@ -67,6 +69,8 @@ AirSimClientBase.write_png(os.path.normpath(filename + '.greener.png'), img_rgba) #write to png AirSimClientBase.wait_key('Press any key to reset to original state') + +client.armDisarm(False) client.reset() # that's enough fun for now. let's quit cleanly diff --git a/PythonClient/land.py b/PythonClient/land.py new file mode 100644 index 0000000000..549d6a090b --- /dev/null +++ b/PythonClient/land.py @@ -0,0 +1,16 @@ +from AirSimClient import * + +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +landed = client.getLandedState() +if landed == LandedState.Landed: + print("already landed...") +else: + print("landing...") + client.land() + +client.armDisarm(False) +client.enableApiControl(False) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py new file mode 100644 index 0000000000..5a03fadbbe --- /dev/null +++ b/PythonClient/orbit.py @@ -0,0 +1,180 @@ +from AirSimClient import * +import sys +import math +import time +import argparse + +class Position: + def __init__(self, pos): + self.x = pos.x_val + self.y = pos.y_val + self.z = pos.z_val + +# Make the drone fly in a circle. +class OrbitNavigator: + def __init__(self, args): + self.radius = args.radius + self.altitude = args.altitude + self.speed = args.speed + self.iterations = args.iterations + if self.iterations <= 0: + self.iterations = 1 + + p = args.center.split(',') + if len(p) != 2: + raise Exception("Expecting 'x,y' for the center direction vector") + + cx = float(p[0]) + cy = float(p[1]) + length = math.sqrt(cx*cx)+(cy*cy) + cx /= length + cy /= length + cx *= self.radius + cy *= self.radius + + self.client = MultirotorClient() + self.client.confirmConnection() + self.client.enableApiControl(True) + + self.home = self.getPosition() + # check that our home position is stable + start = time.time() + count = 0 + while count < 100: + pos = self.home + if abs(pos.z - self.home.z) > 1: + count = 0 + home = pos + if time.time() - start > 10: + print("Drone position is drifting, we are waiting for it to settle down...") + start = time + else: + count += 1 + + self.center = self.getPosition() + self.center.x += cx + self.center.y += cy + + + def getPosition(self): + pos = self.client.getPosition() + return Position(pos) + + def start(self): + print("arming the drone...") + self.client.armDisarm(True) + + # AirSim uses NED coordinates so negative axis is up. + start = self.getPosition() + z = -self.altitude + self.home.z + landed = self.client.getLandedState() + if landed == LandedState.Landed: + print("taking off...") + self.client.takeoff() + else: + print("already flying so we will orbit at current altitude {}".format(start.z)) + z = start.z # use current altitude then + + print("climbing to position: {},{},{}".format(start.x, start.y, z)) + self.client.moveToPosition(start.x, start.y, z, self.speed) + + print("flying one orbit") + count = 0 + self.start_angle = None + lookahead_angle = self.speed / self.radius + + while count < self.iterations: + + pos = self.getPosition() + dx = pos.x - self.center.x + dy = pos.y - self.center.y + actual_radius = math.sqrt((dx*dx) + (dy*dy)) + angle_to_center = math.atan2(dy, dx) + + camera_heading = (angle_to_center - math.pi) * 180 / math.pi + + lookahead_x = self.center.x + self.radius * math.cos(angle_to_center + lookahead_angle) + lookahead_y = self.center.y + self.radius * math.sin(angle_to_center + lookahead_angle) + + vx = lookahead_x - pos.x + vy = lookahead_y - pos.y + + if self.track_orbits(angle_to_center * 180 / math.pi): + count += 1 + + self.client.moveByVelocityZ(vx, vy, z, 1, DrivetrainType.MaxDegreeOfFreedom, YawMode(False, camera_heading)) + + if z < self.home.z: + print("descending") + self.client.moveToPosition(start.x, start.y, self.home.z - 5, 2) + + print("landing...") + self.client.land() + + print("disarming.") + self.client.armDisarm(False) + + def track_orbits(self, angle): + # tracking # of completed orbits is surprisingly tricky to get right in order to handle random wobbles + # about the starting point. So we watch for complete 1/2 orbits to avoid that problem. + if angle < 0: + angle += 360 + + if self.start_angle is None: + self.start_angle = angle + self.previous_angle = angle + self.shifted = False + self.previous_sign = None + self.previous_diff = None + self.quarter = False + return False + + # now we just have to watch for a smooth crossing from negative diff to positive diff + if self.previous_angle is None: + self.previous_angle = angle + return False + + diff = self.previous_angle - angle + crossing = False + self.previous_angle = angle + + # ignore any large discontinuities + if abs(diff) > 45: + return False + + diff = abs(angle - self.start_angle) + if diff > 45: + self.quarter = True + + if self.quarter and self.previous_diff is not None and diff != self.previous_diff: + # watch direction this diff is moving if it switches from shrinking to growing + # then we passed the starting point. + direction = self.sign(self.previous_diff - diff) + if self.previous_sign is None: + self.previous_sign = direction + elif self.previous_sign > 0 and direction < 0: + if diff < 45: + crossing = True + self.quarter = False + self.previous_sign = direction + self.previous_diff = diff + + return crossing + + def sign(self, s): + if s < 0: + return -1 + return 1 + +if __name__ == "__main__": + args = sys.argv + args.pop(0) + arg_parser = argparse.ArgumentParser("Orbit.py makes drone fly in a circle with camera pointed at the given center vector") + arg_parser.add_argument("--radius", type=float, help="radius of the orbit", default=30) + arg_parser.add_argument("--altitude", type=float, help="altitude of orbit (in positive meters)", default=30) + arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=5) + arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") + arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 1)", default=1) + args = arg_parser.parse_args(args) + nav = OrbitNavigator(args) + nav.start() diff --git a/PythonClient/path.py b/PythonClient/path.py index c9d7696d9d..a1f6e515a5 100644 --- a/PythonClient/path.py +++ b/PythonClient/path.py @@ -7,7 +7,17 @@ client.enableApiControl(True) client.armDisarm(True) -client.takeoff() + +landed = client.getLandedState() +if landed == LandedState.Landed: + print("taking off...") + x = client.takeoff() + if not x: + print("take off failed") + client.armDisarm(False) + client.enableApiControl(False) +else: + client.hover() # AirSim uses NED coordinates so negative axis is up. # z of -7 is 7 meters above the original launch point. diff --git a/PythonClient/survey.py b/PythonClient/survey.py index c3dfa68c8e..cd8a5514e3 100644 --- a/PythonClient/survey.py +++ b/PythonClient/survey.py @@ -18,7 +18,7 @@ def start(self): self.client.armDisarm(True) landed = self.client.getLandedState() - if landed != 1: + if landed == LandedState.Landed: print("taking off...") self.client.takeoff() @@ -26,7 +26,7 @@ def start(self): x = -self.boxsize z = -self.altitude - print("climing to altitude: " + str(self.altitude)) + print("climbing to altitude: " + str(self.altitude)) self.client.moveToPosition(0, 0, z, self.velocity) print("flying to first corner of survey box") @@ -56,7 +56,6 @@ def start(self): trip_time = distance / self.velocity print("estimated survey time is " + str(trip_time)) try: - self.client.enableApiControl(True) result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, YawMode(False,0), self.velocity + (self.velocity/2), 1) except: errorType, value, traceback = sys.exc_info() diff --git a/PythonClient/takeoff.py b/PythonClient/takeoff.py new file mode 100644 index 0000000000..b772564d6e --- /dev/null +++ b/PythonClient/takeoff.py @@ -0,0 +1,15 @@ +from AirSimClient import * + +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) + +client.armDisarm(True) + +landed = client.getLandedState() +if landed == LandedState.Landed: + print("taking off...") + client.takeoff() +else: + print("already flying...") + client.hover() From b05a9f0874e2b2f99dcea84822f3cddcb6158861 Mon Sep 17 00:00:00 2001 From: clovett Date: Fri, 6 Apr 2018 03:47:46 -0700 Subject: [PATCH 055/121] some ramp up timeb helps --- PythonClient/orbit.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index 5a03fadbbe..4d84bdd867 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -78,13 +78,29 @@ def start(self): print("climbing to position: {},{},{}".format(start.x, start.y, z)) self.client.moveToPosition(start.x, start.y, z, self.speed) - print("flying one orbit") + print("ramping up to speed...") count = 0 self.start_angle = None - lookahead_angle = self.speed / self.radius + + # ramp up time + ramptime = self.radius / 10 + start_time = time.time() while count < self.iterations: + # ramp up to full speed in smooth increments so we don't start too aggresively. + now = time.time() + speed = self.speed + diff = now - start_time + if diff < ramptime: + speed = self.speed * diff / ramptime + elif ramptime > 0: + print("reached full speed...") + ramptime = 0 + + lookahead_angle = speed / self.radius + + # compute current angle pos = self.getPosition() dx = pos.x - self.center.x dy = pos.y - self.center.y @@ -93,6 +109,7 @@ def start(self): camera_heading = (angle_to_center - math.pi) * 180 / math.pi + # compute lookahead lookahead_x = self.center.x + self.radius * math.cos(angle_to_center + lookahead_angle) lookahead_y = self.center.y + self.radius * math.sin(angle_to_center + lookahead_angle) @@ -101,6 +118,7 @@ def start(self): if self.track_orbits(angle_to_center * 180 / math.pi): count += 1 + print("completed {} orbits".format(count)) self.client.moveByVelocityZ(vx, vy, z, 1, DrivetrainType.MaxDegreeOfFreedom, YawMode(False, camera_heading)) From a71248168993116550a73fd126334b2a96604e0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=96=AF=E4=B8=9CStone?= Date: Sat, 7 Apr 2018 19:53:34 +0800 Subject: [PATCH 056/121] Update build_windows.md Run AirSim build.cmd, says: Newer AirSim requires cmake verion " 3. 9" but you have " 3. 7" which is older. --- docs/build_windows.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/build_windows.md b/docs/build_windows.md index ade8991378..f22e7f9bca 100644 --- a/docs/build_windows.md +++ b/docs/build_windows.md @@ -36,7 +36,7 @@ Known working config is: ```` Windows 10 (Education) x64 VS2015 update 3 (x86) with VC++ -Cmake 3.7 (x86) +Cmake 3.9 (x86) ```` Even though cmake 3.7 says it added support for VS 2017 folks are reporting build issues with that. From dc2da557d4f62b2dff789706967a93caed9c48f3 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 9 Apr 2018 19:36:18 -0700 Subject: [PATCH 057/121] load vehicle BPs from json settings --- AirLib/include/common/AirSimSettings.hpp | 63 +++++++++++++--- AirLib/include/common/Settings.hpp | 8 +++ .../Content/Blueprints/BP_FlyingPawn.uasset | Bin 154302 -> 153281 bytes .../VehicleAdv/Vehicle/VehicleAdvPawn.uasset | Bin 0 -> 119544 bytes .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 8 +-- .../Plugins/AirSim/Source/AirBlueprintLib.h | 1 - .../Plugins/AirSim/Source/CameraDirector.cpp | 18 ++--- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 21 +++--- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 22 ------ .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 67 +++++++++--------- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 1 - .../Multirotor/SimModeWorldMultiRotor.cpp | 8 ++- .../Multirotor/SimModeWorldMultiRotor.h | 1 - .../AirSim/Source/VehiclePawnWrapper.cpp | 7 ++ .../AirSim/Source/VehiclePawnWrapper.h | 1 + 15 files changed, 132 insertions(+), 94 deletions(-) create mode 100644 Unreal/Plugins/AirSim/Content/VehicleAdv/Vehicle/VehicleAdvPawn.uasset diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 304980339e..d81bcb280b 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -48,12 +48,17 @@ struct AirSimSettings { } }; - struct CarMeshPaths { - std::string skeletal = "/AirSim/VehicleAdv/Vehicle/Vehicle_SkelMesh.Vehicle_SkelMesh"; - std::string bp = "/AirSim/VehicleAdv/Vehicle/VehicleAnimationBlueprint"; - std::string slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery"; - std::string non_slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery"; - + struct PawnPath { + std::string pawn_bp; + std::string slippery_mat; + std::string non_slippery_mat; + + PawnPath(const std::string& pawn_bp_val = "", + const std::string& slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery", + const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") + : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val) + { + } }; struct VehicleSettings { @@ -196,7 +201,7 @@ struct AirSimSettings { bool engine_sound; bool log_messages_visible; HomeGeoPoint origin_geopoint; - CarMeshPaths car_mesh_paths; + std::map pawn_paths; public: //methods static AirSimSettings& singleton() @@ -227,6 +232,7 @@ struct AirSimSettings { loadCaptureSettings(settings); loadCameraNoiseSettings(settings); loadSegmentationSettings(settings); + loadPawnPaths(settings); loadOtherSettings(settings); return static_cast(warning_messages.size()); @@ -371,7 +377,7 @@ struct AirSimSettings { else if (view_mode_string == "NoDisplay") initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY; else if (view_mode_string == "Front") - initial_view_mode = 8; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; + initial_view_mode = 8; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT; else warning_messages.push_back("ViewMode setting is not recognized: " + view_mode_string); } @@ -435,6 +441,47 @@ struct AirSimSettings { } } + void loadPawnPaths(const Settings& settings) + { + pawn_paths.clear(); + pawn_paths.emplace("BareboneCar", + PawnPath("/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn")); + pawn_paths.emplace("DefaultCar", + PawnPath("/AirSim/VehicleAdv/SUV/SuvCarPawn")); + pawn_paths.emplace("DefaultQuadrotor", + PawnPath("/AirSim/Blueprints/BP_FlyingPawn")); + + + msr::airlib::Settings pawn_paths_child; + if (settings.getChild("PawnPaths", pawn_paths_child)) { + std::vector keys; + pawn_paths_child.getChildNames(keys); + + for (const auto& key : keys) { + msr::airlib::Settings child; + pawn_paths_child.getChild(key, child); + + pawn_paths.emplace(key, + createPathPawn(child)); + } + } + } + + PawnPath createPathPawn(const Settings& settings) + { + auto paths = PawnPath(); + paths.pawn_bp = settings.getString("PawnBP", ""); + auto slippery_mat = settings.getString("SlipperyMat", ""); + auto non_slippery_mat = settings.getString("NonSlipperyMat", ""); + + if (slippery_mat != "") + paths.slippery_mat = slippery_mat; + if (non_slippery_mat != "") + paths.non_slippery_mat = non_slippery_mat; + + return paths; + } + void loadSegmentationSettings(const Settings& settings) { Settings json_parent; diff --git a/AirLib/include/common/Settings.hpp b/AirLib/include/common/Settings.hpp index 5a24ff09be..bf5fb960c6 100644 --- a/AirLib/include/common/Settings.hpp +++ b/AirLib/include/common/Settings.hpp @@ -128,6 +128,14 @@ class Settings { return doc_.size(); } + template + void getChildNames(Container& c) const + { + for (auto it = doc_.begin(); it != doc_.end(); ++it) { + c.push_back(it.key()); + } + } + bool getChild(size_t index, Settings& child) const { if (doc_.size() > index && diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/BP_FlyingPawn.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/BP_FlyingPawn.uasset index fbee2b5df327fe637c767f035a79799c56895e72..0f4b2616d7668aeb476e00f70ac54af5503c344d 100644 GIT binary patch literal 153281 zcmcG02|Sif_y3KugeZj&N>pU6$WFHGd&qj@w&Zr-_9c4?Wh+8Llp;IXl0DhWl6~K0 z-?Fd&YebKFd!OI?|2$9g`HZ=)b7szW&Y3f3&RkTfC*$+Aot+&$EEo&}6Si9pK|iNW z(uc>dKUu`@WPd4~FV1odq78GjrxVIQpviTuPkt%2#fb;edTd87o)?epxBEgIHPL;E z4iW>4_FX-`DP`+J(BT|+v5@B)5kzxb!GIlxS~)Zd0X2tUmC#Qv4Cu#Rk%8JX6fhVI z)SfyCO@{kB<1-%&2I^Q41`Kt+;o7Zs3z?8vTjd+HdJn;HFb>E%1S5uF!B%f_!bqSu zCMH#}D@;lVBm!=YV7|h_&biwG2AfE^^4~f@!*+|1_5(}sKpcz#E4;a9lkY4TWT_pX z${FaLRoK)@+0>l%I?@(lVP%T4v1Yxlpev4aG({OJ!0l16Tf-JpwEwlEf~0~7+#F#A zhkXjSxyE{+pDfxCfwX3oHAR^!+QJP*&=xibD^>+7w1ut`!cbQM3AeT}HGmP_!L4Qg z--fA~nA#wI44meD&3^tr2Li0?XhTN@6Gv-P18Z40glY;$!j#0b!p{A^yAs+4ZN~BN7+)%xlN})cu&;y_+`iBe`)@LEeFPE~PiCci9FQbM&?p-#G*D6mvcENK92*G-8$cL|l!MmG z8n$!t>-D36U}Xh&1dD~RvT=k-rVp9!Qf+O}3TSIBcIJ;xlJ1f% zERc?p<`%X{YlJ*X6mAYTM!;%|sfmDSePL)SN(f_9YY0}=!Vp?!$i>VpppA*LfiYHg z3sCL%`p#7`H8A_$-wM8q^J>S+>BxQ`H3ZTS_7%TV1Zb%*0!Lj(dmv~grYID`5N63jFS9$@pO`mItxcgmFvW9xe8BF|P?WWe72Fi1f`*&wZKYAT0;t(psk?3Hl}t6 zm`$WDobs>EatMSWB-_@?7@=&2Kq?`OtYLvETD(BPAD#z0!F`#K{2=9L4A{j83ua(| zA0s4eO$}l7I25X<4pJq-QlsEVRj@8dN0?WCj3RLLAH$R#p;a-ziLkN;3+iEm$Mlm_ z6-NsMjNn*M9~3BI`f_L+QzJ(uQ)AQJeJcWaCuBZo9A4u$AwnP)+Rc)X{op7A1Wai7 z&1c}SfGCTwHh~GG3buk3*eCq#W9}T=V&7$;kw3hd_)z*o5MHmNZEVoyKY1=+kLzpN z-?Dy|D<5EHzPqyWb_gpgQ$vKfEy`fmt%#Q&Y3^$KQxHAU(yK)b?CS%c8E%ljUWB1})b2eIw@U>kdf$3T}K3P5XWWQs)W&IAva z|2eSb4-LMLVc%X>1qx zwE30LXd768-}A#fe~tLT&{YzXmA@$_sw;Ypo%5Oq+m$P#SGcZUVH3H^DK2`Mom*V= zI;S}26;TeDKa+ALu-P8w4+*=tJPL&)AP$L|T0`19?%MHA$mn%oM==m$|BChtv;@jP z@t?^&JkU@Oz6@at3D|nTQeuYRkGMnVEN!6E-n+7a3Bnw%E6o9WSNfC?>^~6s5Qd^? z16vSRB~eCbSiUat25_Cr4@gi9hxRXs zdGRj?l6L3pfH1JNL8$5@tc*bt@w4l}$_JSJb_F^>M~yfVczJ=M_*vll;z%^y=7*g& z;(ND1#{-Tjzks_0wu8786l{Kqh_47ny8-xraDEPmq6L&`V7u>jlR+5S?4<&-m>RyP z|C9dnV9Sbo%)nE22SX=;$^K|~HzE_9_P;WI9{s`0{S8-R>xGN{T3~ zn}AU_pu=D<7Meo&6sh!B3D|x6XXZ^fl#N2$Wp}6X?;P0zyANXiSe(~QoKhfsNOOP< zsw)CVBKOsRt%SY$1iXHaCkEUEW_|w_B|z9C?swkZEU<{C!;Ii)_=Ty2u!fES7%!r6 z25iJV?*7~)5H>JQtDq-f8ZuCp{_{Syh7nL>HUNA7$gQW-7ubPA$s)`l-`us$vXUjv zU!+}a{b(|C!0f;GQh-CwZ3A;1ci{tm^PTwDin!+F3GQMbY~Z4B8#rv(Ju4W@6}nc~ zA`#G?Xg3d1fZLeBEYAAE!Me(#?GPZ6{D|qWsF=JH021K;u<+dlTqz(L*mrlXjmd|S z_6bnDQ-_sn`d}U)Xuzz*rxD;30z<4}dD{_4ug?-am!v=D?%0SdqfRbpC;95)goCF(Y?_CCM0FLF|loe=UZ2^ZO!f_i% zA7Hh;PEaKJZltJ7uMohYz8m-Z`A6RKS`*B6_cCXIHJcB1+V|TL9JxOam<9TCJ@6vH z*eC8!7Us4|VFQxQzlp2Sto-!A(SGS7hqf|@E?;;uZO$O^+zs9cD-|?|VY?ei+zL_( zrt*^S8L%&~7aOdV#xD{q_3qoxTm9jpU+I2Fb{HIUd}F^O`_CQwcs0N5cZ5!yb2uvd z9XWpP$mD!>bidkO5tAzcca+dM*O*+>Nn65>8l=u4f3CCdfFxyEqwvgc``szmJ5@y8qZ( za$Y9W-^YBH%?3MVuKx&J1NPC_ezspsxOUhsF9UQ;`v%fj6 zesI7qONntJDgEGp%diq@Kf!|oepynLF6USf9`Fl89}tq1MU-{H62aJ; zzLbLi4J9`Jas7o$+qZ(%gLt+U!1wkQ++Ri*#p5#U-AS4nqu@5SR*3JhLM{&b9LN`x z_sP3~dx$*G24sHFXCezEbU^@jS!hns9qs!RVJDcBXhF={O>BPT=D*}gRcp4#foCfN ze=*p-yB*gMQrXqv2k9?+q2|TTf=~wOzq@t8=@v-o^hxW`jj;0)weQx1=A$ zk32THK$V|O{Z{63CRkh*G#aU5Y5~ihz$pR}e~CxbrXklr-tjYeFM9oeaI(EN1cLfs zz4kAG+&hCK`(k%fH3cZNTf->mUS0<72;Ic2eq%=2Og$LTSkPyFkM z{N^jVd-B|bz8OJ&8b@`=5O~Pi-ST>~DDXf5UVC8=lAC@ZSFy zUK=z|;N777YHPrw-R1nA2fo^Z^1E^1stC&OcrxEvzvVgp4KMm{c*%dmOaB{Q_TTW{ z{TE&hwAiDwOcf5T&{*DLY zBq+b*?bG(#gSzkA>UTVF0D!XJ$LISitOv5^)4k{YacO(+U_bcnJ@1dp-FxqbcwzsA z*9Gyy_juo+=e%ep#QQ133HV_O*?FZns$oXZrva>mClsMcVeB_5KAMu=WRw$leQB zo0Yw1uFZe0&)flBfLNA2*&zQi*-J86w*JBaPk+7bbw1Dl9whQV@|~FjG@u*!!U4)f zsK)L-!C;}V{fu{y@P9P_3(loI83%E=_y6+e|B%1W{$Fsk58xQ>J$L^B9NpjGT*v(P z?FK)9v!Bbo`4@1e5BLEYgmH5Z2N!BH_MUmM{?h0F@;5N#ckA;j&VJkhIF)-iQ-1(w zKaK*N{X1ypp?|K=<^j8bFK9@p0xW=&y0=gD4*$D2)O$E(dpIt87t5n~e+K8{FF3x0 ze+H-g7aZKMuCp`y)40!TO}_ ziASIPGdSRz9>B@m6HiV5XK;9b(WmkJpTPm&n-A*a&iH3=_qTt^PB&jsl}g9EN32XT(d{23hZJF|m0i+>0Q`xl%} z**~KXI7ScZQzG|gZ~(?ZoH+SEgM;@Aj-SGx!2!QhJgASA;y=e3J8&+5b1oaI`{zP2 z^bCr+(m%)f`U?)o3qS$w!tN6cMyCAl;{@!<1~~iQL-17oeVj+X!O8gpI6=R`F;M;Y z^?C9e9O7U8b>3c=|JwX6KLDM9f9+Q=bMOpeHfTXi1uckqpan4rv>;}H7WhAC!JY#x z*b|@yXFq7c*$i56c7g)rgYylXJ%CQU*B*mfAmcbxflMGDyaRk76W{`w`||+bi$On- zJAuCh19E^2Fdpzg?gsv>6ySk!M&dgs(=sX0_^jDwLl&{RB53K z^k9JM1*o2ZD)4DWsGfx?6I3rjl@6-RP-THC_}#!csIoy7;GBmlkVgwuAQR{Y}pS%44Z0{MUrXh0_5?_&TupnrQLf5e9#XAs{3=<*98lr zfiZv&>;vAxd;tFbJD3B&-G2x3I`|Ieey~57ADGX6e_)S;?_i$$c+lg2EnvQ!Pz8Q* z1FGPDD-2Z;sER^W465Q#6@uz@sDkekV9e#c7W_#fc)z;Wa_qHN_FC{yK>(j(uch2; z!Jpp(y3AfHz1K?awNg+E{2F}U0pDK0_X_YW0o?QdmpNX8`e8!#3*?6_PzBFTP&;_w zdEmhFuXL#U_X$Fi#J`CFd!QgEL3o4$n2AvGhN#j$bNAnTYFWK8g28BElA^*Y4j2Vd z5A3KU-)dz%=D$39V`qo*OSGxuou+fRRjJ{cacVE!M{n>PNoPFk%&x0nU6qWvx~jWf z>*nI-;OyYw;MNvFk)YemYs;lwd{^WI>I zx0dgiQ~Vm+xGym9u+$km%f1|0s+`85OgZ$7b$(_e*ug%zf9Ew)>4X~Ra(Ni8A0aP= z)DV&|+tnZf^GP4;(<*K0uWs|VbVs^%w_PuuA6U@rE8n<2gxSpFdM{MGtH78h1SnND-cCDJmrncSckgpX-k z4vN@5j+*CLl-J1BkBDQJFBak9y1nlII;Y3J!9w+(hxrk{+{jAbw7K&+{sWO+#-=P< z26*x920|6dQ8UAI?3Lc`1UDlyF11UbVvw5h!&3oU513$IH*limLgDOYnqzGNx6{au zB}O}UR1aephUcz0w3waF3?`a7G*RNM8`;{J=fc{{f2yfs#YAlVeuE4VSsfvl0;`Qk3d&cFg>(^)5jMVrNkzNBidc5_zTfY1h+}C-q{I zglL?`AEvLzc}%+D%AcZKCOm%0M1;t6*f)N?s;`Q4zC;*%q~@#yd*PQe^E!pCOGBOB zuERLxgP{Tqsj2g4HMi@ur-wAZiXphnJ1C#_GoRXEisO><3&D9VHN?FrA}~%?eDw4Q z?6R!-VeB-Hjsk@3IG6ZWOR5ZA&p!YBtaao@#i^PxmSEF|)PBd*Gcu7Wg|_8_Cbwxg zu6xQw)2uXz7%jSBmP}v>8HXLaLUYHTIoxmU%)4uk$sDAdubqGJCVk@5jgE>Jo__Zf z3Z*Gui^N%FUv*AAb6-j3&XW99S+vQOP6;ne|F<5=w4GJYw7GenwAd@71HOL9M3(1% zI-Pf&B0dTeNq|R{_c&5)sg*dOn+U^|Yf&WaT`W4Hfq>L#NvljL*6Ov!Ynzt1z=kuI zZJrJ(SumzgS`K)gHuRFv-d=8VMj|&R$f)Vdh;P*1ejbB}3kc}!|B@iL@o1(&)XpQM zTwWy2e*krJkX=H|I%DvSpNEb~KAzu=t4JS_S;c35Wpkz6LnUYCFw8opzXTjIAI;*z zTkElRBKe4>K05WHvF~Np1|wDvsly^yidEGUH-d(s`)yxVrMniYgajQ_DLPwRM;OI- zDvCK+mW;1uYYP;a7ntQQQ(`$+a@`qeb6(;qoK9kHTC$UmcgV>%lu8TODm_o`O1V?V zLKVi$`_kL|P)rEw{uxxUnLWO114g`__w^Ec`~CzJ0a-oy0QIcq4t2D9j^7K6dv7{< z=t?B%)wWAkPCTN18%Wd(TRy57b0p6#`+|{Vw!br0CQF@`snKxC+y1%RZV#MIbP%0r z?fdKE#RZ)SpHg#Wx+3JAHq?{N<2hQAoF^B!1g4p=twLU)DA!qZ=W{+!XpQ*hBe+sr zv$5mJn;>`1EgqR8)vNLR@V_Bk>Syn7K z2_ss1mP_Asr*heKX{Nm$Q4ACBc{}YYF0uSvYdS<%X2MB51nm-%=HD72zaeEGovVF& zHO9B!4Z+7(;9&fy#IjEjW*8&CQ}T2>eY#3)q~T)W`*Oy@rO_r9yR{Uiw)N-TaXn^g zWHPJF5l5$My42?$v--bZh|u}O?9K8zD@QgmccJJDD|&L;Zp$ObrE{yMje4sm)n8wG zqor>#XJDFh%ip^GD2JPBNkM$RY5BV^sqd?_O_v-eb7W;r-EwO)OQLc+6SUYnS>3u= zT?ZJgU*{ID(L_E`XuIQ(vDxf&h_biW2bRLBj?kTSI!caStc{N=!HT__rJbnRIn%~j z;Itl+qBZ%YF<*korM7YOzVl?xW7ZU_hKGe+a~D&ZYBxezsj0?BR0H2nJ2@}fB{*B& zqEn7!)?E)}+F|T-MI=az#6uHZIy|kDAU8R9GJmVJ{6e(AbWzPtV(LzQW zlfR^q_vg?mZ+T^tr8dS+$OtBg{)1;)c(fnFF!>*Vkgq2}2r#7}XE9Y^~wT86yG0p@WH>PlA zIpG#TL2N4jnq2J6>DI&Exn0xBeN5S!RE8#KzZ@o~@-;u5$eN};yO^-$Q{uJ>Ov+K8 z%_wz7ZlOE*^JL8;C<@Iwa(s2eKQpB)jrNp5&cS3i-mK}C3x$Z0+@guLqgPpnJH?l8 z6@AEySG$#w<}h6rkK9zKn@>hJM^~%Hsb}jZ%d(~vsphsXjoYt=B*?Kt|Ca=u!X|NZ=Pa=EYY@8Kv`IFLd2Bt;%v4QQNs? zV9<4_gj{4%#8q-JTsP{Ow|j^HJcTcY*PX&c{zV_zmMog))_jci$at8KiHp?+^{h=g`eT4%@(fti5{c4tV@MtQg0?7Xj9-m+#`(R_^=A*!n?yXXDpaxQ`S z>WcGZnZV-PRUG)euZLjwY7W29p2%U2=hDl$JeRgL@ub(4{mnp6EIQnbGRi;CJj}Hv z!M}dOU!&(sqVjsGSsKS;SFcr;ui9yokga6#IK-r|Nc^3}G-s3O5N0Ry+~SPVwPfA3 z&iCtw3pwUYW(6RNhl<7B(aoNPYg?we#x$+w_@@ms7Ef#2)_UqBe@WEs>tv0-)0Aqy zUY=41cO&1re*7-?G*JyMH4PTUc*FKn6ZL}ICp^1eB+k6&smhTpT&iJ0ExnjkifWxp zY+4@Gw)Nf6*a~O4){+p#WQTaD=(zscJgO&k%6KWXFel0SbyKnNioJ=Z(U3@d@mhOc zXpQ#xG}$+Mi^Qh&=XKDk7bi|uhOV8a;LuE6ev#dCrzt1uGi%|M39upTx_Vb%_l!Mk zcNHAx6*x>j_w5Yry^N#n*#wig&$sPjHYXZ)l25qRKCx>__{8sO+V-BOoUwSzX(?2= zI4x-^)U!=s!CcMT)pTVpW=l0ez-nb!a0q?JT+1|iO>0d%)+vOwcR4AmwY;69A$7AR zdz)!HG+#f=K5biyl-e$#IEabmZC5?C-sMcop-HDrHdI|ECQimLg}xwkF%U87>sw6u zfcdWQFgX!vALC-$Yq<~|5y&MswaI2Lhc7fH$e2!(r7bQOC#Jn^6L{Z{-bj|Eoys%a z_YGf1cV#Y>CtLZ`vZn5culBZ@ExRTizRpI9Z=>sEP1sJp?NVBinp+c-rq1M-+^Nx? z*s?SoM1o*&51(OCkjQek6QNP2D2joIxA z?-$3uGSA`pTTGCrc!eXB0C#_jbZ4|wubWKX(%l2C*XCK;!qm58(Hyoq-GUN>YWR} zq`hC9k6$z+Oxs4~`D=zVt5H~GD+*lcY%ohJ@Vt5Vwj2gO#$tF$1_rEEDQa0TiAilJ zg-LhYc5sYemjXkS8a`dW=ych&pnG17Zdy0vuzgq4cwW_uBR_WyVjyS zH6q;Z>u$AD#%GzG4wuqlp4x6r-h7_kki$3KAa{FlVU5Y*{Lw<~)JPHo>BgedAL>U^ z?<^Y7^Td}g%MX(*y?Q|u)s}$2l;d16Bz=KKnpxX%T6dD^1Akzma)(*5&25K~wy@8A zSvA>S68W~{DWx6e>&&nFl5#&MId7yivSMfTRIsmjJ4C{oXDdDBaZh{lg>vD(I^#L} zb}gUqTjq9!kkN{Let~Re<4_9KL{3@3^ywo_xGO#_&9`E$mzXt2dQ>C+YyvwHR#J6ZPOR@WC9*>!fw(L=U(j)fQH25hPdHW#619;{?D z@GLn@#vVR>YdoLOEo^}iKTQDHiPBwSf|3y3_0-}Z$jeb`#R5$`pVDebeUydR2i6RP zh=vq$&j{tR9l6>}(a91~)K#Um(SVxImq8`ajBt)nGj5t)G#XLPwO=~dIg(SPKeKJj z@uH1~LZR+8iJw#FXYq5(8{$lbS^Ooh&t#6bbX=uIvfA-lQ4-dC-Hw_U`MV;*nYC5)>bWR}`XF}BSTIVho9m1vCoZy8>M5MH+#y)j8c~?YEea3A6hXYXC(PG-O2^&yAu7oOl=K6>Yu;;I%!L7 zDZ%mdD>p*CNdKj9Z9^ec&$XHH-aF~^DDLQvBd0s+M}&K-;^l4`3zFx?Ds`h8xzj9b&s>opk7V`B^V}CS9G$$qdqQ6RpQd8Izh*P87MvHFNDyrV;OnlKQ*L zif{KxZ3W!qN4ogV9oyN7JdUqn<^MX1B;<{}u9UFiELZEy3$eCi(e1AV%&Wyfg&8&F76U2yPibY=u7DGSx zP2#UG41F(#GjvIfdMm|kMD5$m!@Q8**IP7=keS8{v&ZL*tOUCi7jolT^xthP4Mhn~MRtFiz!{VG(;F=Wx9vnj+S%Ko|wR&6xn(5=H}Ie*^u|`?>RD_3{CRaQo1>{*i8;l@Q>~&D<-3i>5(4B0QsGD8QL@Pa9RJ&89ji}%5!2Ft{ zX81ajhXvh--1q*J-Y?+uxjN%Jlzw==%Z2X&@ULP^Wl-Z>({8EMFT`$A5yFz06I5}1= zPCQ-Y`YdC(oJ2v(?2>Iw^O`r_KZ6kUUUvRw0X~(XLoCe{vtJ2zysp5KD{~{3jiIV# zS{pk?qvOj)XTQGB@z=OQWyy}}Z-xz$(nMj&T$jO3c|p+VHy4;XG9A?(ZrH+A zb!W00PG@qn%VcCFj{AhB%5W01$Xi4(Z+zWo1 z0LN9mYDs@tL>a+7CG<={r#)w#u))>C05b;mEO9xTbUf}xYrqYqtMJJPHwlc29rh|5 zK2dRd-uozR$&$w+#RZ;3Wus3-(NUx+eWzSHYB|IN_EA65Yw|oCB89vB z!d$!t)7#99wmck6_@X_MS|KC%D6nfz0FVcH$#&*drmO$Ysa3!+~U zvud)odUr6^bzEdXA>zDTVS$NT6ia7%N_ZI9kbZ zzr0{*Vo*G3HAA2%E7N))g9HmFw=cw2yW*^lWUE^nUWwm;(y=Q6FOza5K4YKv$hqO& zL>fc$-m=+fLH(#p$T6(ekra$ql=mNe92JxQvH~kxOZK~p47?$mmC7JcFg`Jghw={n zBG#sJA|2*^825bB_7~>3jybHFY{>?&t{gH!eskDX0Y5YM;$$?Cf@3T z-%DX%ub6OO;iMSv=WC-z$Cc9qb|};Fgkc*PkA1KiEE;`+UzRAHq+S>=`}(H+?&GI} z#~(kJZ0+7s2=6{+_)6Xtr}t`IMfHV~8jH$9dTWEjLuTQrU3oPjsXRM|x5c=Tns<}8 zt51&#J?9Ql*JxHHcUiAadK!Ullv#X;In7D*+-5dh@xIl?y2mD@q+`v)!ZcfaCQ%U| zDY+2_Gq|=v=?sB{n%vjE(DH02a)?vwNN%Ky8F=f{z0e3`udok}qfWZh*`49lmD8-1 zP9dltMHqFyS-!s16Q^>)a%dDI2kvz=X5%;mYtB_KzENqGx9y?P(sAair+jZvYY&^# z>n+#IH$Q%=@2yygf08%*w#j?*D(XuN3UnmML$r}c^Hr(ZMbDqz#y+1Fc9gBu)AGZ| zQ{Ng3F3Nr+RD91xT8^aI%1NRl*b@E9AQl(pDq7UOYLJyMQ0G7%*tKAvs?SqgoN+^i zAq7Q)=1d=Y-}5d~CL{My;fjQ;!gz&u%qb~!s#v1nv1i-Z6r~0W$otGVt=GCKuFJ8p zr9D>FxNiP3jT8TH(0LEj%V!GQ8RCzR9VT`N%HVPFvOr=!XUPk?^Db-`wB=`px}O*dN2M~2Zj7nxx>R^r`NemN zSRJKG!qp+OdRUana_gq#t7^nj{~_Ju=wfOei@9;hKuqxuF?X!DX&Lnsb!U(C2ESFf z!J$V`6-#nd>{z&$r2N??WUEzEcBaOO55b<{ETze;p)KxwW)wk-=hHZtEeX>RSFSaf z=EnJJPpsmPhHC{Kp?_UnVyyVlld|~iSI(po?15OUcVcShh(n_I>Y^r>uVvgykV=Q~ zcN8vMy8WpeU%KY@oy?cL@6Tq$zO|N#e#&dvt)Q22@`9Y0*NGDbQ=AVOpQfkUyfu*t zDAFIT93l+A_ZWxMx}|h6nR`R-wnaEy6;wU*eh+3*l9qa8LaKZ={soz#%xJ749BsspvM&&YdrF zytr_#kRt!(>2qf}{lYI)oKqZ}T=Knlh6mPhf2>l>AfXbEmJ%oEb`#BMLjsRy_?(`T z*FJtUCM7J#nia>!-&y5Cza`P+&NcgHG3vQi<$h%JMgdND4a|p3W;UjYGn}iV&s85M z|I!WDF(Io{5v^>3hP4kQdK~JY2yyd)(PINbO5y0|Lz#5AOv3W@T5l`n;Kg(?pH-%) zL*PTPJ-pd`dCi0_Pgh;3aw*$CHGIS%twb?X8_sLLuWXUtQnMN>7$_t@?_K{vlJu$O zr_zrzY{d6jRX^z;KRzNE=0!S6v2j@W^V$>hsaS)ZDeEgVl9;md1utl0i zeL2w6Z>NsbW8>tn!}tFd;7Ywy!6WSLJDFLAy@%o#81mYgLK>zNaNlB#hH`iH*i5|| zKHma+V(3!ORX*y-!pA#9BImDA8Ne<}%?%$X6)HnW+^rSL#4T)Zwpxf>I(Jzw?`*6$ zR#DsqVXXw^Ak1+8ZSraN^W157_1`*t5|=->HkzT&!8@VXLG)rIda9mWAR>k9B2RK7 z&iTXmgQY8V@`58<(=E_%wv72LJ^dxu7FOjx5tjtj7u{;af6gU*kErbUf+XyC-EjFU z*x|e~5~NSFmVtNADSvdTsfUY+AM06Vf-$br(VCJL5C9TZzLeKAohS@K`FF8^#ojr6EhQu#ott2FV zwPMCe{Pm$KZ_ZTcx|iMaSF$Y07udUwo9kbFpWYopV1@KsEVO7@Nx4hDI>p}jA+bOn zcOfytuaca595I!}KkuWxMH=Wgw`zIwTs_;z6~nXf&&STFx#x1LQq*#qXebxem#dvn zC@3w9qgt*e_>Uof_4rgUV^>|x(59e=4wX^ zD#JHIj>%bvZ%;I4l2nBo-@?X6*)`vzk?;`_oC;kSZ?{bOG)yHE7y$FD#6FwMr4cS# z29H?KYs{dgoxh8Rb%=ylO%e(H=isrF#cX0$k@3x!j#eEK`#8tfc;Boq%d!2QkdBS% zxtR;<>ge{i{Hb?`eWvc{WlM&jPZh$Gq|ZOQVMAAQu0&uYJjtW8=t4hPXZkB2)H4|` zYwL@GbvLdTsBy~MoZ!uy$EGVVCku{Dbkm!m1{CHzN{$CIzCup)Sj%nYaW9h$i^wiUj?h|;@3s)rZ zcorrmqdiZ4#-POi`1k?q1ltYblzz_YBH*bG!k^5+@^pNfZ8;JxbuKjdTkcT^2P#WKDYXYOS zJD6`=n~hGS;f4z{r9Qo#sX-jebwa)_T<>^S(D<>-%!M7xzBWsiPY+e&w-0j!e^bOK zudQz4_G%Z2chqb8c)aL>wIn&7Wn90WvgRAtqpZzJ$6kE%<~;tmROCts&6+!ZA5DbW z)%juOp)>3OOHSuJIn)<|z1@2RgkCRtTooYsrk=^a8hLqX!2heqNK}~K_F3AC!!^8= z+}F?AQTcmECd{9DW}WPeX!4B7!H$|Aosh~PMc;9}#DJna8UB>;v;#he%B2bWzT1Y% zj?3g11-py%w9v?z*@*x%Vnx1(-jUP`ag|(NPdio?7LqmH%REKu{rl(Ytb(suEgIb= zKSq0=?aO2L*rMc(aQB-q<^GQ(K0N*GD*cE|g{~{f>9|4$#OGeGJqQnGjj9&0Q0(C3 zc+q$@l*~u#>vK^~bEn%qCnP_M1`$~>gi7$=$WH0i{Xj>(Ng+s; zIkh6ymy~MoNM`0#HDwKf(ve05ju=j#4jhKV!MaA9QZ?7y%jly`65iB`ntDuC`kx+YfSzC>X;mW3fkn|kxSow!6nvN?z!8GcpPZF>NJhKzE|Y)TV3P$|r-vV9&Z zk;&%p&C>oEqEi&#Cc+k0JV}al1h+R!MM`u_RYTv7;a25^NqIe9G4sGNu?eC+ek}2t zJenZ8C1!R|@>PijuFj#NJGH|2rVXmG^724LJ`8K9Lc! zIj7TH)hxiCC(}(fW))pQ&JV){A|)hVolBbkraIMtb&{OZ{C0VaI6M1xa!+uPpi=O+ ztoi2i(eqqI)j6bC0)+(yRCbq8C&y(fBRq$dcsRdQ;*1=3^SR$tNylCtSm%q>?+`t= zHRnt9jc#uBjLOvlwvn`Nw93zELQ8Hhh)-5=*9-}=_hYdvOyLM#doY&R&$so8(7K}J zs$KwIOlZZ8zTS|5_Zc?Y4!t#BSC3E`R6MP^;yKfi8nDft!-?ZhC7VG$Jzr1hud0Cg z^~qMbHS^;a22GR^T*jTls~crpR`lDowS6|23YfzIvW3>gpOE4-5`ijStzute9+|%F zE|RSbM)Md{^|rrBo}elBj5cY(7nn^CSLtOguRNrQDcEw~Ve`#rYg~CT=UlmYo;>y! z%W!!Y(z78#rvrK4^Dc#0ZC+tk3SImd;`Pm+pt57dJG%I7L@>MST`K+nK#4r?6k@qyxqoR=Yz%9 zd2v;drdvScarul4Bd2lYg)J?y`=#{I6|zB+V8u6ZXp_n=0;Y228^g(kAR6)L%W&)CR?5rreeNZf!U`YwbAO0i*k1iSv?+UQq_O)xH^xLIW=kn|9}>lV z+LSI-J}@o$y#HaZH^=Pxf<==gigS2L9vo+uTA*xE7>PL;7|(?c%yfNQ zXyH{^AAOl-&bx`b_~_aUgicEBD;1R(6CVKL| zZfp7m8=eyixT!y?l+0Npb&Z!+Fp=9t`DybdR|~!iRS&&*SbMED>aQQYGdGw&t*#`1 zdk=5$X+sp}=d1_nyIPHtA$y)gp^%>X>U}sx_G&*5Cx4pb@;P^LIkqu3TB^hA;SrCfqnm0&#BPEvf1zl2ng7mmteRMH)DJTyKWnXt3Wje>3d!YBfWG zqYx^D)ui}bJ*KxD!`l_>+rhrQk7UJZy{a;Y&k|m=5kD8}op$GQM^8k;`_;Q>q*bBp z4ZrFYPtMb>TJGf5-rB|&r)oYL8%j7VzJ2jPF8VB|!D`&qhes>zgT>rN^LYbn_&tfNBiR~Br@yNsCx}K zTIP!mnME=tMep8S57mF6*R=FC^O55nFA+>EO5SNYiFw159hEeM&$x(WiF15EsdSE! zL@Nxdkdl4HBgK|ut3D@~Ggnyd4E>smZ7@iTmacyKKK}_Xmyx6o3Xh^byBjbB_ePvj zBk1^a?hOs*GrUBfPb;(z2JT#G>R&b(KODw?))SS4HW-291%FTzcTSGJdm>%O?uk%$ z#j6}*1J}0UoGQ|69ozuxLrAydV+FJ?ue;`T8d{&>WS};HDd)`jduzP3#zZKJtfT}k z&EqV}p!!M@HVQU{J|GfpSZ3ei+4z2azg3Q!@pnmRJQ>L?R`>Kgy0gFF#pV}=_@Jc^ z^pXf?THDIG7FK6Vxnl*kpDus#_#i#+Fn;}R*eRx0hTd1}9*EQT>AgBhR~P<_bLiIg z?Yh(3e#*l{fgNNiDeoT4R}%4K!MaJ*V=kRNGNZ5m>5Wm4slL7OBdWYw^)t5ERqkgi z#E;wxxFEx4tHOt2%ZIRhX>43dN|8m7b6eK1>*_;M`k8VmS)Ai{K9rMj(ud!yMj>~tlxtO@G<7t`t;c%t=89Abp0VTk=;~&~ z-A5N59Ooq|WDqXJM9WmQ;}a%Sw0&-hD=B}9oKI>9#n9Xqt`MJZv8^U24!QO)&~oj5 zTEWx#hL&Zc$HFNj10H8LV`C&93s1foKlPftU2EO_^J;}SDajXubeqz(I41+=7TJu? z5vd+QP7)cbhlHovk35pqJSra(C+;@t;Zs32JFQBlQFy$lLI}SYZe*2C8VVo!%-rh1 zGRWwcpv~(~)aG@0^d9dxy;|Q+oH2}_L#W>X>9S!E(kN~}A$BPy>PUVVK)k3mCMUQzK}vyjn!;w)vA16JGU z^$kfPSRV5{*YLR$pH6lm-Fv%B?W?sG0?xag*40>6&&r@TTkb-hTC5Owt6}J^PjMl! zgzjdar?S~6wpo`)#;n&^!_~A4Y23=$O}ZiiyfMD|f9@qCR?)8sLHmnga&nfm;d4Jk zTvf_Bw!kZKa;@JtFlft{#B1F-lQ`!5%!5~}%c(fQm*ldS$OFB6D=9s{uy=5JX{_56 zCr$-heS9yqNwju?_RY1^!v+HhwZ@ZZEjW_TIltCBaiPN3{r<~^x>N=Rk`|Xj499bt zbzTNJLmrbzY#wbRGMAQ16}_m{M6o_vPecCRxyOtoIcjd18m&ifZN4*im|LYdM8Bfr zEQX7lZlnlZyfLw^*qtkDX{@zKy?z-*8(vv>GQh~#_=R8JvrBc}-Lu2@BnD1*)dexc z%yQ*z`pj>pZtxpky0;j0_W96KSCv$U!HO{7^+pI?DVVL#1_NQJliL;Y=z^NH{Ugk@vMgz8==V&4@`<fYxZA+h*HBzMq z8$8G1&`Ew6j8}^Zw`g2lN@arncvJrYRg0P&?&{H$*>|Ul$UMjRRP>cRc`qr(`w;is zcfw`I8{n9$GE3k1lKSn-}q(348?7I;&XIt40imCgQgm^v=RKqaBA~pTx;meoA!4&qB+N{ zFKCy;VE2}aKZUVS*tZ| z*d*rj)A-&OpD&iIK1_>}JwlFm`oVlqoC9}2H9dWG1YS~%-k|l%hWldlyv+6MJ=yOu z6*+wUzh&{BNx$djU9b^!N}JJeQt__D4OYCzPd=@ks}43AnbC^CWzOeq|w1PzoLy<=^o@$?JGVsK!_Kzn! z9ML$oPHFDIa9LbYf3@298}jOWg3j;_hnmm4L0uClq8rj2{OEK^;p@HrikmpSt`Fh_d{_EK(CHypy~g76 zTfga(b>yWST@UgM>+%Zop6x4g%3OS}fGoWK%|UWzM1uya0{;3_5u^W+Dm62_Ri6tg zx;;~uIG<*%hZ}ZqohO!8@*ANY4sfV?n&d@uv;WPiw#g`7J&nI)t8Ph#;Jim4soA`o z^Eof3PzG*!c~vJXM-rb+c5*AMXv%k7Ct>sqhl~j|T-&(=@3L={-n-`FUeP;BqIe(f z8|TI{9#k(QTiI3ZelLBk)R@MUTUZD~h?R!k_T=!j@KRw(M4qL7ELm-#lA|=6|GTf9 zDO6&~I{yz>K&Zb4((f>ck?Jr~W5|z=6j<3{tR;%D@j<>5VG0)|ZflI@xfL0> z)f-W58;mCJ=e8fGUS7UGynO#Ae4c+uUT@EO;RCtx7seLk%Qroi_K`^ggAieNaWJ8{ zZ=uYG5NjzMzHxp1y_bjBhVB$BE%PD7;m^_OgqSFD8(r+O()x}rvnB-{t zM4?tEAi@k7pQ}d~{Y*~!HzWUjoab0}J3@ClodhvO47qW9ls4;ALf3F{v!zTB-je3EGaOEKzYb!2dwh*n#GNB%TQ20R#lm_M$tfBh-gwipEvGGxS8r<1z2b@%Wa0!k1-rIQ zJa0dhC!J1ow0~1hUSI$8i-pd{#dxy8qMwU^ZdF0Ge@_oT{`sfYwt@xT#-teg2#H$5 z7RcaPj;0n`Cv6rXsQe>X8Fx)j!hq`4fdZFF%B1NRk&usmb}dZ!sje4+Soi|YEO6vA z>Bdj72$TtpyTBQczUXvOcVaAKPc7I5)OgSgfIP;oiU;0Ll0i~@AzO?Sl5OiDS?>Q5 zvp2%gKNqF7wfH^|Ob z4PwERqMfn^vBxIf`#FwIU%rTNQn5EcItcf;Z3GAJScJ|a zFwRTxj?F;i!yn|?Jnob(2o?gSD1{?#)?IEnIJ(c5ltj#fU*$_1F$>4J!LdnQ=B+y5 z8gm$G*QS~&V!@r+=2$&QqOq~U^XW!yV$P{rxMDi{vT>{vOJg=8jCJhWrpARBxy^Xj z!#h4j`0@H98g);YGh{aGrzh|<4>UR?a$=PnVB&E*{>az$GZDv`E-r*rj!ER1qsgEw zd-t(n7I0m)v=Xc3K8`}k@ZnrNg%<3jNJ3)ae21FJ?Ar?`h*Zfgd^?WkoDv+a6WD

~7l@hD4egx6)Uc|7q2y$;5 zCKJqy;c#H0HcY)lR&8c2wP&)LAowH0FN~SW8&9*TwTKQI?4(T^%sXQnVKaG?hig8W zB3~vX+)tUYb3Wy_=8&|J^$ zwJAAoluYu)AaC!A9V)k67I#N2!_yi2Ss+^A-+dB$dE)mK*lf^3PQ_ywqxLV<q!TLv@yAM?gVip#7Abfr8E|6^6MY zuwY4If*@E{u_pI8!^!cqfvOmzZ!HInMT!#b6F|^1aFP-xS4)n4z0YQ$Gx0cn?6^2=2y7;L=Q zkFW5Uvy3kB`rTm73m=zG5c*x@oFpWGdyvuf*s5bO>8WP!;r8S6w})?B=_GfIk^Fnq z$7}wZ0OI|D9`Ur$1b{+&n)L{BPatd%Nfidw!MOgBgX7eL!rfogUjN_2P+(d4J3obeLe${3CriJ1fAeg5%yWo!?)C25p~q@u`qP&BB6M#$>hzvgYV*V*`PYU|#c)*z; z*lNr@fiX5lmh3y<1GYcHE6>7X z^XBB8MNMB1?|%I=_`oqCG+~@4g~rzFjCqcAz8vRzG_p~@v}NTutJFh+rUQ z!buMuf@2`?n^HS7HllRVU}%b(42N~lodpXQ8~8=Sig`ZO9^2S-?4zK{GIkKWnY3UC zgfXNvuh7Uzer5Gp58Jqsbr46`))O=G{oIy8>VyKbIcnVAeB_6HKwyfW^y9Y+k+Df= zK~0}b1`D3bRPC5&^~wC_y0> ziwDweH?d_OwsYiK8#rS)i=vC>O#I06g%|z_qu-LVJ-uzjF-wZ>Gc0^+#Qxcjz8yWF zXpoDax?TD4<4>i6FAinQ&xfHBVz6DN&T5Y=JdYUx6rc9eQ8OELXgTmpO+&@63^NBS z9b1g2ei=w#WP17MpZ{*)EX*2+Er!Iq$Y?WpJSkL@01CUaC$e@I|Jfp(lsq5QxQwm` zi%B}p91BAe3XyDjx$-&Yxk}_nDhXhfm>wIE;d+{s`600){B`;?~ML+v}r*i1mw87vKi zIw{A(uOE=5)0iX0@&=WK8H$By+8DT8k?G78mPw_5KF*4lGhc9t9ef^6CFh~Xg4Dba zRm<4%1YQ&0H%_uaPW)U#&n6z;dF~`l^5m)_#GsQZn9 zicj4*EK?DS#SR0DPk7x)gg=H7y^|Id?}(zo&9@UZ+4V;v9FHbCJ|n|t(G*Yvq(Rgb zjDh6ac#LdMOqeFOBN*&x+BQ7n_zhXv9%+-{)9?fVh>r4V%$ePyn16sxf5rqh#gjqm zuiYu~ z-~oi=$dU&VLs8#Z5w1z<_=F`Nzv)Xt-}F^x3^_wmdHMcbzeJVqLIkYyQ$230BtvXu zyyN9~##vv{oh(Gr#h^{-O+|{nA)ECfFk`~<_#sP?oqpKy0R6#JY32c^(CHA7hdhAO`t)2k9qdIY-k|DD-saSkAPMrYKSGf2(lhcz@_EDo8 zn}LM;N_~P-mPiz9R;!9*QM$GODBhMcAG7hI1 z@fBb`>U@3p`D3mCnk>$lWb2D)s(7LrXSU^e<}3Kq?T#0Cxu3ruu8F%>&`-~py^AkwWHYMaD($|v&eKM|&yADfSx%yH|nKqf`;MSz~0UZ^?6IDYIH zU{0U43*NEl6&#DcMKW*tu0R$O&WQmS1{pKj;HXeBF61N6EwpxYF*rZ^MY%;+S3Scd z)_|fv^Y?pY&Kg|Xw)BbPvBxreipBwMdU?NHrOJDdSb|9oOlyC(<3d+}$mdu5lXCy|qmA_L%! zMM?9HKQ<(12jm1kvdBrSgLD80P{6k}%V+ZQ?I|ThgPPq|-<}dS3CV^e9++d-0XS+V znGdr^930KFFqpR57=)R{L{E0X@h?8){+B67ZkY7oy6wCXM=a$zh*b3hD#e(M%Yva} zFGsrZ%_##Yc+&JgFJ?8tWV~p3qZyl+0XBbOMr%V79z)Ud%OhDVjWH8aGTvBlknc@^ zdS=UC<#r!mvvFHUz2rc2jhp}0y<+2|M1Bz{@dU_uB45gpzsHbYA{j&HeBvscv5cPH z-S^zBR|LqSC*Qc_G7VKcWHUo@ENJ7g@F}8k%HbmP$9H{frmKB#Ly+(slq@Dv$TVpB z(O6KRxR3Ee&MfoX5-S$-4((X{=|G_so61e0JgnwO!j6m&n#caF(t$jX*_en=G39fX zDpSv7>p&lq26KS3IHc=~TCWZ{WQL&F< zvoPZ$!P|xd#+xb9W0G;S=W6W-jNU+iwUz`^XQDe8V;MoZ_~=9`R_tf7F<~VuT5uAB z`NGTJ>=ObqyllgWnRsfx=xEYK4(BE}vXk6Q?4w#4QtjU%AG%K!x-GLE7L!M0HZaSu z(P281K7Tx9l<`+rL>@BwH}Hf*9D4Io&{@zi@b@8z#KkeC=|v)&juSmOcZ@(|7nzb@ zlu_dZI^rG?e`&ZG>zI&pmW|_^8BQ*^QWGtIK{?Zejc~?91pLS*D|E*MgxYKXk|VSf z__Rt$?N6JSFS6u^mbyUl5pb{a@u)iLDPH#)K)^+d%*xD>u2>xmDs)|SQA_KFCLv-l zCOqVzp)`U3%*7=SJ0&ANZ0|!$jCqmq)|~uP4*`7?8+e>yvN?Bpvmqtc2VD$cbPA48 z_|G>eUO1wGm(4F}h#aY4F_GNR(czDL_?Rj^&@77smX`e3zyhyW)OZSqf#-n}L~ya0 zxpGP}PsO#iuP_MXqXsqio0(=vH6C_+?6A<;DYFT3iqyR#lL%Q=cx;{V(MfYWg9QkJ zmeuX4n z@X&W$?h8SV*>4L3`w$#Im{qYwE~QWE9ME}ewmD&RULo&~=D4-dgFDLe%OCc;isVt;k3}*2pAtNw$ z&Or%1?_G@ES~St9L1MyiWn!@V+e`S?V|0;LW2EAGc+*0VvbkwL7$b| zYrSRRaI9lyu(+AvYI^6ueBbAmPA;HllVBm8iya!gE_Q!RfHnYnk=y))!6Oy3B$3}GU(|iO&8jP$na>0-Pb5H{DeJjVLP96os;KzW#51bPX zgXet?^fB)3cak+;3l@(hqLAgMuM84ra&*u=0WB69Y07N@i2`8CG@to^oVKq3A$F2@ z0~_%SKj!7H79HzuJatmsK4zJefPzSk&m@b*2u&}YJ7SiNz-E;{{$_&W);#h=W5Ys+ zV>=>VR7?VOSeRCWZaF5+6vkkZp2a(KZ_YBV7IEtjhn_VmKo z6rZ@DjeYagv75YktVc~%uZs9eIgpUm%ku{l67a7n^A%z}^keh3ulS?bJR*|UHXvcf zIvZb5^FC6>C9k=dU=#zY#|NC~=>dQ6jX%KQWg#J5a#6B^=aP?MJSze~|BI9CTZtGb z4leTeY`fvy%8AC&)T$0Sr zj>QGa1R^4kggvgNqsi)TxkL#Un->MwiYxCQBiq$`3W+Ar>7+!?_gi*GayNVeX*E5 zkN@i9r>{W9H=%CDAKs(lxcK&>zpKw=(71fp$4WUFMGgP?0gl3NiFwkgFAa}}CpbxD z7%Ql{yFWrDgQrn2mMg;1i+kcRkq3WTtYDF6az|l)un{r&JSI{=7dh~0dt59%XfT9K z#bWb!NJK%_MKSxM99&E1f6yMVe6mw{pKB^{& zvFDZjMAt4v(7L>-AbW!mvc*xFwH+!jn$VtGqfg$BWFeIVy7oFWhGIhQNK)iH9zphe zR8|w0*Ntb1pzXN%7S+5GnjQiQ%#K_$$zYEAF(^*~vBSCwF+ePI4SXiUxjc{=v&62_ zgfV#(Ky5Jq=*xr9C5Pw)$h|lZoN`sbuuyGZ%_;6mhbxT-41$A=AU@_4p?>|3I|zoS zSn5(6Ob{QzCCgQYq}sTu3`$~fQzS6&zN$3z#r5@QK3whGyn-kfLU#xnAiyy;)^e)54- zFJ*M-c$^eF+Pf7zG1_fllYBFui$tVr^Fs(#~5IR*fa+sgstXAF*sLZ zIX39x`xU5e{OG_r7Cf#{G@;47<#39C!FNoPExMAiSv8tpsHP)JL%DP)*uoWx>8d>! z^}7Hsdg94KKZ6j607!g;OtK6uxqyQQXc8VL_-t6$a_O5Z0VT<545Vn2OuISWdqG;9 z0W;w81&@gXI-k!b>F$FCwiBefZ6^vX2r6TT?xe>o;$y-@hU||#R3^XpNJKwkv^^o2 z9rDHDh=mDxf9UzdASMg*pSq%RMG~0jls7}Q`G~S72{ZIYF3woTH_;q7aoR?5NMz3>if0@XBb?^@Rtn8j%X23CB*aV(o&X<@C4FH?^;WT;Q*U5 zpOB#ItD!KNf2V-*A$$Dqx%`Rpx7pmi02woI_^-;kk(girKvEPNu5wX&*5~ou5@GL2 zzfJe;`yY}a=e*QJmD(LPDq_Sw7-Bd_s^9exS#+Z0J0YBM=w|^+M{+_`j@4#UnMF0h zsha%YI+%Jf1eo?ajS0i~fH_hzPn;lODmk$?N(3hY35v)V+7)I5b}CGdWTC6U@QlH>jf*LL7ADrZ0Ew-> zqv$baFcAksSf;ZGF|Qwgd?_4a3Doc$M^lj zrXP>DetE}$eNN(da?hVDsA8GjAfK+pUt(??#F*jBChoinpGj;P50ISdNXVyn&H~ZX zYQAnezf6=(SZsoHL6-C-4ssZRsTOmrkilgd-6?D#Im+WjFgx~$lmE_?^TaD1A6YVX z3=Ag79pmOPP`*4DrJd6-aqFXI$>Qp<7h+-P*z;y3Y~NmiXJEiJq8dQf5tdA6F44B< z;Le=FK#zwWKhb@m)#Xj<`kR1yNTL=4{mXyYe?Wl)&qWP88-f#!aaM{*cGSXW5}3yJ zNBaK!+~N$d+}7mh+GYQZkL+R0M`TfqPnh;s-LdX~ZF+)a06K{*!xMS$m{c8Y7ORa} zpO&wfBvK4=D~A6e*oh0(M{G^tpFi?YDfjCfC*e*Jj=wm=0Ez@2M}04R;hu{eCZUN& zI#kGo7cCvpd0^%I0Wz|x^t1W!zF z3dHWSPIy*hYAG*)kS@1TiNPBJa89O-gHOPE}4B~gvFRz?43!P#z z#CH|&5xH&#-q@|nS`b6Oo%q5)k{3@oz7eLj2Oi~KnQR(*JYyFlnVFEtML?|tCAr)K zs}HuG#!~#>Khfa!SH&BNCXhrNq@BVs>3M`ehexMMyl8 zS#Mqpd*OHj5TkfAu5ObxM$EzaI6OsMfQAkr5RZ^6aP6cTSF#SV>B2S2HY{*Zf+TnqSz<7kqs$} zpZF6qK3YsZW%^PxaVeg>bqoJF7!&sB^h$$mLIsmk33)!RZ0cn{fgN2Lc+&5r@Mo;^ zM?gHniOxcjQ{i-x|49eFij349E-G|9hm-?f6rhcB(3x*GX=SlkT?bu!^qoH%Q6+xQ zd&%;Xg+EM*WW!5th?A_5Cw|;NMNPpT2ZkE({z?4IWBKQ_OVUNU_6ccljHpU%FK!v* z`!DqKpRFi{(S6pjL0M1_k0_@j-Vg-L#euV(nZ{?L;gQr>{9d5)C_4x=k|-|DqUD)^ zmpCpyURoar(GW&P6CR$ErLO%v{a>5CPNvr?OL_gNEO?*XJ zi$qt=mNy;8DPCmwpmJUbt>I0AFMeRxNkR;ti+V@Us+ga7&O|1EzN zI7x>?5~)-xTHBVilOwvFVUAEhp~Mq)rNjDD-}3?f44h>I@9D22tiL@cwi9fzIF{q` z7?3^>cN_8fPm|tQq#;^a3=$2T8YKVi;cr&LmGRAeHh=+?3>Y0mjP=yzuaq*`O!T`k z;d8R#p_Vrw4f3lt#`q^&K0Q#!Chtt({ND<~(LZkrYIv-S&qQMU#T;LsXHt@M`v(sB z*2aUL{^?EclEa2$WXy3y|G;BY?or>OTJZhPD*U-W@&KuFL5P2<89B4nhwU+fKlt4& z@W~e>u>vOB(&c3AoZuYunIEfmRzWt%& zWIo518X2}-$5$KDlV#I=qNuB)uz;%JU8>0-B9~OfD zV->-8u+ec2)L^uQ&zO60VTk{rm4y#oi%SqHegnYrP>$kQWKa0TKo^a8jwBO`wL5h3 zE*TeZlCmeaeLo3~G)1~I0#F{nfFH@g77xtMm9wMLv4f_wBsn zkgy!~?GEa4h^cTb*aVpOV>}Tu-HP&Axzc#N>RW$Y_32wjO27Cf;Bobm5m@ZlIL2qq z_=557u-*p=4gtM9kJYz5U^R>Ek_glk?@Rtq9>4)navb|-BH<4^uyY1e3A9cBcVxH< zZYG7%B+nlvVG{A@wL0=yTw>a=-sE?KWq}sQY}u2fOmURNy2OQNubF(rhoEYB46H0F z$%@ZFu+`8%t{V74$CI0ctCvMCzx{s+fRUW2@}wjtveheL7KI~3Sqd0ZQO%ECa&BbH z_QGZoC#&ct8>gQP4#Y_4jweNbq%#;SM*cV@9wV4IQe#?nMQ8COo>9jiILZea&ZDmO zKmPnv&wFzF^{kKTu)%GCaqyW9L$$BI(#2{lG7cI~`BE=nFec0eXnBK}b{8)!a^x$r z$6~U9J(b*gjh;m%OF*PXbBtzf=_+zugQ6{c+4n7m2(95UJeUBBdDdGnpTGuFVlK!s zCKTIt>%_pm=Md9*fSDVotY!do6>LUCvsxW)NBS|Z=ZS-f49%ZEemwlgfBd_2^xVkn zB{qbvi-Jx73@pAuXfPz(7axH$IsDCFo5uly^>&Gp2wK?8YONA7u<8eG}ON^D0Jl9R<~!dCG{H$uQBIoOTwp1i`Z z?Izg2876s~0Z0u`%$X^yr3_5{;CRMDh~YOG&Q@w=O~A>Oy7=HcOuWe98xKqwaQ_3w zFK}c_FP4Bq!zn}X=0Vd21V$!c6p3Av77V~!=%a$(#LYM{F(WDr8nVWOPE5oqlIWiN zu&027*F2cx+l&5nV^J8VENy*a@>32NQ~Hu)u4Pjm;>2UfnM2$P+BT{ro7jjDS#SLE z5W4!=1n`@Dz?V2SR5{l*u7000h~YT%uy0~kyr_DJ(IrpDRL8b~mPCMUC-MSQ@zqz# z&g~(D9ek7~bHe^x9LN+_E`E#9#V%2oh)>5(pHU%zE&m6s5g#38%5#h8-}Q$_^=HZc z{O1g}f?_OHeClA%3FE~_B>CabNi|oUI6(BTR}!2eIn+6tvy~@6qboU?SmcJm(LWUQ zFB5|eg#ZXBUie;DgM~cf$&>N?ZNXV^(nVU;luT9i_R-w(z_V7WK@K@LQfJQF{$2Hnmfi67VnUOq0x}^;Qa5#Y&dTW+?d5Kx)Zu{j}G}JS9Z(0m>}ht>B@&W8g$(EAHek(p&4NNi@v&k zZlIctsn3zX&77Y6NUw7e6jFXH(ph+H1mw|jc9;_w(iA*mj;HPZF>b*^&;Am>B58>x z@oF;{Imi9~eqcM5BTU}2aS|J!0zjCn6cg9Z=~9kgJVe8cC+Iku+W~u@Bx-=;6^URn zi8lcgM3R(6L2&wIU;n){e^f`(r4zk)h#6zZ@G;q?Cox=ABX-=eFcTojU>fvF#sLkNF*ju*s(Epj3Ix$xhNK0OH|mXsa1UH!;y*2w$Rg!hj}9K?L_H1 z=hXOxxaPz!@}qHOY+s#7=z7_AgQaPkvM-q~bdQ7m&|6pgY&`ggtWU#&GvB!hJ@XS& z@^k~^VM8(V7vAenoyJgbo)gqd>|E|)yqjq5sg82~0JIytsVL;u+P`oPdU$u&CXvA%ewvbd4QS6ehjPgeC`&--Vfr;iF3?4F&?1L5%p8%j_MHXdzM1~GK zVRk=f*!p1b96z@n0ph5W`$WJ2$u5YGzSpy_8Wbi4Gg30X9YGwAM0YJcM}-4wc%rC@ zynW)0NzFth<)Yh$$rD@UNvw;2glhm*-U#VW@oud)S9=PJAu*?oo)^7*bS;_WZxrA= zUim0p{I0oQx<|*-KXt_bm%jm6)1KCTXv8d26mo#=l2}vXBmNn0#xloCN$7;Yf_BE_ z8^yPqpjcu2#aV*WD_@Y}sw|p*QR%RSh;0Kx-1)^1 z{HGvb`Y%{9W~^(}N5#7lo0M+(mK#Tk2lCL@oL9%?(1zJWK6~!k(uNlW5 z1m{3>Z>(dNk9C_9z_~avw|oKf$qPX|oia`^SJ)u_@sIzMjivn7llOL8;|Vc+=f4Ic zG{FeSG}%$Zg8@Os$0_0=pbR4SLVWk{=2Gwg03ZNKL_t)YUvgpm_{JQcMJp4Y!0hyP zj{AB~A_gsX&QuDM+^X=~Ts&o$8%{jqgfTCE1PD|=+^L{%I>8}>|2RfLkBQ78R_OrX zi%pR@6~`jH;`^6(CaLU@=m8l&tdgr2F=)VRpV(PATog~F?BL5fgdK?C)NgC+-&jJn?>Vv#1D(~?TM@6f&pTlQu_B3kb|@@4w3^G7GRFI9KGHypd25gUYnv5Gx`O_!G=|4~+TrEzgzi=5} zHYPFj`r9A>reA{MPYe?n!o&@BnhvxWB6Xnf1u`c+|Gxkv#FGf;XI1|4)ip;3CaCG+ z**N$(?8p8)FB|3VaWNqCUKBQu`SvE>a(ed2b~Y?Zj;oAY5qA`FeiXMaZ)NAQ`` z#&O(NKi5heWn$rzj(%EYAstVA`Ktx$Nh3Z2_oupup4E*Dzf(=Lz z9{D&+Eg#mPi07y48gL$@(Z}z3^kdmVqK`>mg&~q%G$5q(xe-@+B`5Gh z&+?PjBs4n8g*W5LId+KG+DAMrHoU;1YUf-jVC>Yk2XkEa)fD;^j^Rd=GBc2C66Vn} zpqnI;r*%4u} zW*qlP_IL}+SA32o@Kx+78>M)*3sFJ6eEa*u-~aw^>ckdgEW40L+OF|~^)Nn9uKa83 z@b#OJ{)<3Nh@V>Id}erjwNv0y3n=e2HEEnnb}Rv4iiEXzhmxQtBMKx-SJdp_3XCKO zgBb=5NKXj0x~tW3juuQ~{;Eq$aMT-<1cx3;r%m2ST0N@&^f@Z={dpg~9GOdIPaTkwxA|UB+B~_<^2@ip!Y51mEKcO`_fa$BrkYV-RotZr>yUBp2VON-sIp zED#RQCMbS9AXPuW)B^P*0bQ|kdx1_y-7ca4j^-eGtUYsmg%?kLFb>=_PFiQtWquOF zGkMSz-zP7%1!KQF{_1!<$q@iBv7z-rH{Wh0)9?`17~sg{igDrg)U}eIIVFr$IGtAh zUGw$t|M)jOTkC%~BHUGS+cNnV-6tE>z8v`8KfO43X)nE1&Ly7t+FOK-f#7B41`M@<~N zmJL8ef-;}jyL+RZ#Y+AXCy?Y~vK|ZaG3O?YhuXgS;WkH7{0C>$ZCGf(^kpjs^v@sq zPbcwzT*Z$gm!jptDE=ldF^DiR>D}>YToyjhz{rOSl_!f_;kgjqaH0dQ^5r!94L*J4 z!8z79IHiS-5%re)XU#X+_ICjA0eSsICRRKqV~m;VjTT$Nwcl(A1U@p(z4{(L9n(Pp zga5TRe-lByBasAQYaX}2U_aR7$;gmX*=34;>_nq_iz4w8DNNTg}q~E*$(#z@7-RGP>eR}!L zztRFPL8G31Q0qAaYbys5m~Z3_mS!98l-ThYd0L35b7(^x2T=Dt7EZp4j)NGNI)$Wh zLohdWkhIslbR-B`qODj6M4-5Msn0T||B;S@60wKIR(Kgj7eq_Eq(@6&0CxmMY64VJ zAcL=r!gKBYWSm4a8AXD!&2^>;e1M$=O=8Uoi3nDKQ7RpV`W=L$e!K{;4~{li z<8Ssy}I4BA&l z)~*9?F5^^N(N##b&cd_hPuf-wkaa+uo|V0)$!yQ zMv9I?L={ABG}()@715#~8%sa5f~Fyisp&@V1eF9G@$@CV^(6@7cui)iMb;;uCAgYZ zZg$C@750ggUR29NE0Hj$;1(e?aDyQtmnI3B2Cg;+zYs_r6p%u~FyfPW&@8XP#NyRL z6HlY)q3)>@W+b$oDHj!x%_%&3a5igX!9}8p+7NVTH8g5JC*R~^6v&S)v?KHi)>p(A zO!Fto6^4Z6ItY9JE;Ke7Cfe2>oP46b9eYN*n=wCcUMQoiW=YcdoUUpXY68jB#0MC! zIPNxZJ9;FlLkRN}SF`bsj_);{^%TQ1@id%Rz>40Ky3}^!z6SA=U7f!dj=iVt>9EH( z4B?D_#N<-*3!#uXMY^bfXvl_MH6B=o7XBn|o~0k*WqI{ZyM#2B!nXlTti;ZD>mgxS z2H%}kso_HOEqA>b27OSzq2bZ&2xz37zH4B43oMA3|E*VXOBc18k2Xj<0RGDtX}LF> zRjDjGGJQU46bW4Mjot=?jA5LBWA_=-1OG&mI-qKtSJ)Ix$Pj%VWtBVtfian`2?}s zhj~8DpljGP#Odq1j zk<^U~gIo)gfaN(Je}gSbtPo|+#X*$@F(W{p~dxEpfFcVR$lK}P_~x|Oc*kDpj2 zZizHt!z1Z2{KzM0!~A4qBS?So-rT(jM?65lZOL8DoW9_yGyl3t%IXaMs0L6X1q=l| zR{O*(ac;fXiSid6hxN@*+5*8Wgm?rFiK6NH28K)j@L+>Ng<3J{xJzrK=RaUt&vIk% z3(3S?SJxwCOy4PoHYtKbuMj|WNobD$P22$RiBxI;GEJ(dWM+l(ABD0i0AyhTBhutF zNfJ}TEyNXR;nL&97$aerNPX-IAKH{nU@t{9T*hr6VOnWVOrzJJjOD|xJwT;vS~n4Z6G_5h{&QNSdD19 zsBwiz0_Q!QcDl9e*3mv}TSut)*XgpIR{L6~3Ofjwe1RM=+hwT`!XIGPSA`=!ApkN` zbLEzisW|?!mrTHpr!7M+t4%}%7USorH(4diDIONEDRWd}zNJbYEkP8Wg2dk5@LD_( zwHV3@F%vwi3WA92n}gtpQ^$kRLj=7il*A&1fQ_q)X1US`1~3r=@TxIXi!18Pg>l-< zYt@P0#h9QKD~o8RK2ujpb)OVO+yj3-DkJ(BH6*oW4DlXdL55?zsvere0y~u^VMTkG zMc`b8iZzW3e=hvTArN}OAc09ulUW6Paq{2x38Be7RM&tV5Sx5J!S?fKxmY15KA;nR zFVHSXYR6W=)S;DmDahi^#c%~l#B6#}>nr{Q9wEds-+0T5AxW1aK=~TnQnMsx2oS4{ zzmUp$fp@gKFr*sk#x_)7TTjH{V)@e6Lv*9ojEUpSQD}f_2!Xdnk?w?T2m$A$Q8=2LqwBbvmSsRsoQl!9qV5dXxjb5N0ncWbx7&@6-T zOc>70f<)sj1enC$R4|!jEC|Q3y=qsg)}}LM1;o3A(ENorUX~>qONwk%IR89hldK5l_LFSL1yQ$n{RLah~WnQKZ)SM)sxZ|gx&reHx z8j-6FvtI3=GL5-d9@BgG-%Ajb@EU=nZwd?|L_H_I<^#exWN8GFajkB1iP?F}uWRtm z^y!slH_Tuzqzjcb!H0hmw;t(y&pL`)56`s8)ELdZtsF+)(T?H>Lqr=^d^(b48NmU+ z#6u=VjKSpAQDYkmcu@{vfKRl6^=?XgV*v#GcxZ}FuKMw>VDQ0rY0V8_;^e32~u?M46;-grk~52F1Ps&d_L75j_Kdx0;c~$mm|J7(>FrlywAN0k+T#iBIDY z*|6+oJBYdg5rOTj@J&I}BEs;ohZ2kIiB0PHd5sp1f5k}I--!u?WnkNZ zIJM+WO(#->ra%X-k@=-jPTYxT+>DlYp4ColpP0XI%0euCxYROrCNz{F;tDgEUk{#B z)2S?G8sP|uJQOH+sRXo~9SDCEGGc~*6qI<-f{#ugnoq7lpl4(#mU$anx=~B0=`&Ar z&CI+uG(o(`nmhv+>jtBu8bjd6yF$bdy$p@4i4&5RmKP$-laB)#z$QI`7a{(8N&J8n zWyG^Gfx$ZCMKL2mcv-bcvjjD80&SyHo?hpUv+~Hwo+BdJP{sgR8z4aW-c*2LweZ{n z*$6YUjBH_z6IU6IC!h1$5H=p!vj;#t$&Y2D7gjneIuN1lj0^zowbu>fjM-%r(i<+A z23~VBiq49NM#Mkyq}duXXeYgRQgT!__~;}Ok>{VR@KdGd7L3}U6&?}Yh`lr{`y=R{ zlCeygu5Hb&K6$t9t($;vzJdYYB=!KDS=b)<%h23JBin5?xOj^&!v&TFZpE^WQYa+s zHeQ)3+O)=Q09k+Hms%p8yaz*zg>`PY6ql5i$eDkGXlOf4xhm+(-OWyTRp|Me%;Xae z82-#MGV$H)n=}}(SSWgRbbLbY>@@$Q#2AQ#f81btv(u)9T zK$gFAZ3FQYNnHnS`5xC*ms(!Slel=d*M%V(t)NvQP5)v%@vHgR_PWSbCP_dk41zW* zD%QU#anTtxbclzuXZ}eO5h==$ksVgdEzCrFMsH79t*0!d(0DS_C~x{Js6CIs_77v9 zd0yg7A4yE^v*BDEzA8|nL*(6Z#s^fmJN%rRokas6Qs77$$CsW=u^Af{I++31l6{vw?D^1#25vw#AVaiy{#8!F63R4B+;Sd>%mZn6_& z;*p7JX|2$>lR-2p{xJ+Qn;;5qjdqhCMe$vH$tj3i1Rd)cUuPxe69B`%7fbyBKgahD z87$k`GiQUzW~Qej-;*a!zyue{`HA!8+2@}t;~O`YwJhpbyLL^PCeHZyc3wy#c z!xU;cR*P*N9|dE+cw_Zpn^DX2t2cV+wR`ssmZ+>7+Yoqf8sE&0vSEtZRo0KKCt8-V z)}h0eG@fSLSRe78Zj~&NB8>)b7{Ym9*N_0wXlTW0CAQspBz7Cq*8abqIszJr6(t0)+{*kS4Z=wt&zItVjlZ5>cpsQF5UY zOU{K+f`P$F01`xM5MqVXzuwJ2$qbAbcZSd-O{AmzM3s=vPfV1V>Eq@2iR0zmxpM*U z@h84o24F~c*_}LnrYz6T#wQRqtX05+vf7fqhE_GckI}KA!q=V zaHb4&50u4=?$Sdw>s^5vH~sX|+adUF>_v|ID+JNOzD+j6;w#$=6?M!Jb~ z89mZVW5Nv$f^r2-*$%)JPz*_kX}Vx;{Tr8gC9q()2Cl5qr3I1q+Wh+I5gnaUDB3;& z#SpED8)d}X-qrMynVPbljj!P>Ab_QXd6(N;Hv~}Q=?G7sHUk?jodp(A7XrpJKgjePnt|eA;@m{}>Q}y6CMGA!=O6l_ z8I;M%sWNf+a4Dms(W8gwWiAAlGR?1vH`?+rat5Lhta2HLWCV=C=-2MrRW_^}MJNoi zLTnwswPo{FTM&9(WsIKO4I+Lo&)zzTC}$i5>hWER3mguK%BSi#aejj5nR4{V;TVt3 zb5!*50v|!)xM&yn^z{!C=SN~ua5RG zU+XV>+@?4-byjeA_!^e-pofMIqCJTwtu(GObA9(7nqP&dM?}xgKt9n$yL#^s0wC1T zPquwTY%c8_(<3lyXe&rj`hY&gM;G+T-EtNRHse}|vOu86DhV-ssL={m~n3Uk;*)X1vg_ZDV zO2Eo|Ehm9Ag-t(kq~alJr7zjE!+CZL8bcWFl`urL$`B!2;us_0>9^-%5f03+l(#}B zQN2I%;g6KheD;B0c%pw@W;gswu9K;aznSO7v5M#Ss*enFqAC=enYGs z=tFae>JR{hkx(s@ksLxf-PK2z3A z6*ZHMFNjPn5S(}lDr0)jju7lfU-2z_O>{tneZcp$@>B95Sv~ z2@zg?Fln`UQPx_bY)N&TFJ9Yi$um1`-kTD*rw#potKBwL; z0hkyNhfJG>RBcleQhMi92L%CEm4-1CfVphQ>KCO9#BKhpY(eLunuZ~3Ng{wS@zO6U z!%X8p-mNUdjGFf>htboIQq{5%L!6TEWcvKdY^Zp~Hvy?8rzXoEe)vP>;fEe6$DcnK z9k?OiD%U9tR0uldz}xP7Lm3_#pdP!i2XNp6$S$D5FZyjlnN=jBw?_46Oc!dqLm&r5 zuE&7k#u?YUkz`^5I|A>$%yy{NY~8lKOij;}LA0CE5tf|N+b_X93XANyCGIZZk`L`@ zySd55Bfsa^c(gb_Unb8_LF6+WH$7DrXW4-WPHZMeC@`!5wKYRy2!?^O`xSf2j_o_j z&9~ed4I|9)P2+6=Ol3R!5RAg}IW!aLY1Z*F`5r!Wn6goZ$#dn^ z%0S-WJ&5sj+tt@Z*fY=UFK14kMu@RNirN?$?2B^FgZB(&?nj9Aadg)5+(MbfPtDUh zTeEg;dCfhq3FE8nKfVc#3c45_b3Pzx33P}YMeX%6Ur2~BDqJ58>M zAR%FC%d!cZ-Z{NHXw_(THVE&!hM<9I-m4t}NucS?t5I!V$u^UD&3es9tFOuk`Q4uf zf^>c8gTIR~z&BH8>{cdt@*g|ucnE3Sz;kRF1QK@#DF)aJ)3Y~!(YxYX@5X8M()tE? zi=Qq)G|%3AH!S_Rf>%i0z&8>{eyd|9JjF%k+du^`)B8Der%G(ZAowV&O$l2Ob$Vu| zeEO50D2I<7hMpJ70}nq41~2TX@%*M-WqR*--F6$WFtj-mqA_hy8N=VA#0mrIa#V+< zIlJKOi6f-B$bAqloEZOT?yK)c!%z6@*KLHU=Ym0oFaQp^$%y5>61zgN%d;G*6Ay7! z;DjcIK+qs%8#uR(Wf6m93Wo)T6w@&7BAUZIgr%)PR8k;}mH{@l^s?Y1uqrZd8^s3U1SA9n}sbGHVzTojG$3T)WEi z&mW^%F^s}|1KmSqdSL==5fXE!xa%BZwVfJQr|>nzcgsD5aM`|X8-j79Jb&n5XitL_ z;@Vx;mfLQ>gFH`BUqeAx>#&I&J@h>Ox0kW?>&hH(J*sSpHdD~Fh`PzTO~-ATj(sPD zR~v{?Lo4{I1y2Tqi8So6P8*Qwf90=(_UZS-zT#q){wW(u=@A{K<$qgBqR>23B(O zvFr#lUh~j|!FXi=Um4I3Uc-dJjQJcM%|-YQ0R|*kj6U{;j$P~ee+6Vu>>dn@5X~Nu zNR5zcV7NTkH-$>43jv1KGL2xEXJ~;A(*zH|=!yG0^WcnQ%36BrC!RgEYB*Hu%e&?E zjQgxN=FS&-@muD-tvS{BojCX``>!5kT^i%1y$>>!ng@sK)~;DwUi+H6%af0Pt@Mxf zm)mc>qg->%c4)U3W9d-Ya@EGzwW;PdG{nZ9&=_j!yej}!*-P-QOq?;=6fS23=~k+T zcomeos&Q1l3BuODwn+rELg6Rv1%4(B4dC~G@BLv&7r__`kkGMjMJzJy9FLe|uqYvw zgDO(>P6lQdOs@#j8Q1P#3VkNX001BWNklSip~@sQu9=SntyMk0OarNkdo6pUiM9wo1=iHmRQcRzKV6Q&_^QH+wkR$a3Ep~G zM)kgk#dvXXwwykFq?|stQqE1zm3|g2oj6&08a{S8&;!%m1|eSqK41K6B0}RI46i@Ex@4-o zARjUsJ|K z;ZcEQoYpmy2O|d@@^1Ig6<7l?xV=d!E;xmYhKwbo;KIUe;4L=Y5MiH$7-*wu2$w(O z>fA$bd1kPPKlI>d%kiU(Gtn+Awv6x8kmd86gncXrjV}lT!+C$@mwUABIp=!E%fhLr zm_HvZM-U#a{u>%N8;ou1?6~@-a`5@D1nhUb{cn|1r>4ul{8#^~?AWrd>ER`G)YS!# zbI#>pwSNib#djDXpjxcTZzt_N$JzwSfQ<`bP+z>mKq(Ml`i^*&VKYi<2&=S%OBg#P zL&N12evMP#6Ii}Ad8wVWH5fe+4qO!w)~B-xYsSVxz&gxYUYbK_PL(qglSEa8uP%?e zR;cwM&<-3v62;Z!Z44$-OrsP_uxFY@lL$?LDbuAD!AzU;l>73FHCeeb+;AGQqX zDS0aBvjfTW?eOqXPe{LNTFq;C8yp>D@CG3@kOX>VcWnPlP7=9}fF;hK==#0)cMJhc zSX~PU9-Ys;1|X3am^nm2WXuYn<2k~K*7W>>ml-n|@#EignI$fVL zN1wZj$|`*y*;z&8gT>c`{ zZT9}NsO1Y2a}Dc@r6bqI@i00tR{DlH4aW1{?NU?QYa+NfBp)>jHjn918MNM;6mi2Z z7&92OcH-e#Us?hq*_Kq`8QKFL3JYBwVsvU4W>=lzA$Ted^-hEw)-d$oIfNN^+f)Db zYsU}_>k+)&OiwZsflxSp>L8%ng1|iX+`6wE*na?FT*~7eT7qGJ8Qd#&ZYj6#+gIN3 z`tL0x*c)P|48)T~8k)a^h=bkK5bT9IT+lY};T!ytfepLJY~c}xZ|GkaojshA8o-A> zkTE(EN2H7iWXJYeFzHuP6HJ)J!IWy!vZ3F^toNPZpT&suz-RA|)pf3Di?J+u`ZZxR{&=@%ik=@0 zK?7l5RCae5$NDf#4M3ot#M-sv_OfOctpPkdnMcA^Dr-d-hUd*$&H>UWYfqm!Q~u~9 zA3$(*mxcN1a_00znPe@vQ$^U!aMQr08{^qmNqux|2)Lu{67%#|s2mq#>U_v|uB%4f z)KAI?t@5xexM;l(m)sVQ`t-&{jp7pFk+I|2(kgZn)ZI)uar$JLJvUtrGc|ds+p%LDjj5EQr_?6C znNdF)-kNL5z?v;mRM6l2e#7r-D7O2xJFY3auiI1J@_lbXvw^ngNRk_Z zh+FVAxVM;zCp#~3BSClvy9NE{^daQeuwXvO1G(e1tqGgZCFfxS2v|ZfcOe5orfM;4 z^*3UNYpLv!kH8(9+q33#?de-^FNI@X`Zcbq>AuQH@pYO&$O<5$B{K|BseQoe3LkMJ@7`Nuj#j_`Z`L&q4q}bV#(@aAt z2$pHI15Z7MW-x?(;TY3XCt^A)#^?>RuQpGO!t6Ox?;9N}V;Gs%ZD7@1G=5K6T;Y>X zKJ&9SlKIs*0_6F3ksob_N}nlXkD3Uw&cSGI267@F7FGZU;=z;K8bb?Pz< zzk}t_hZ%Og9M?B`Q`xfj9p(BveyogazSR#5m8lb72@uB~`|a}lSN{M(aFF+x2lrAO zWdED@?I}kNAJx;)M=8N=noZte*t2Osa%1?^wz7d#>l$}Y~z z@eo>r2_r@FH#{VNb^QTGKp9Krzyz0dq>bcP2#!n|%t9#Cj4F2vw*&Jq4n{`!^RLm- zX{BYDI50uu$T5EO>oD&`s7Y!fvoo_8-cH716&dXatJ##OTen>u>)18E9EQ17*>AaR zU#Rf*&@)W$xi}+Mp@GZEV@J#Bv!^ivPQrL>R%q-CO2PGEAAjAG&J@F9*J5-OgLf+Wuwc-NBy~PtFFftiWgr zipGVNo>#;N;Vo$qG9d*CngiN{T0@Q{QY#3xoW8s@%Rq403*chy3oXZ^M4Y0Fn;fsu zXzpQ)AVM8Jn3NR-)$Bn;A1znZM7(=xWI((t5Y(3Ra~*(!WEfkF`yf0f+cIlESel-= zEYt7WwWAz3a5!Lj@pN6kp;uks=$^9u<{vCKzV@d}@5m;!KXy{68A|kxbH9iCGamT8 ze_SS?`_1b8vhVlYwWl0A4xJ&ms@&ARVJ%#9?TzKf-u+|LCkwpraVS}r{ANaWc&Uxc z8Hrv+GT69GicH@P`I@bjA!Bq$7P82J}#cH#}!@kIehpCn_$k-bDxi%{?V^~m2uKA^T8u! z62^2*yC=Skt=R;Vj5rm|n0|^L{V=_Y=islz63jWQ&4YuiUni~B`T6;kGDwBauq4){ zuh(C0h49U}DhT%7c4OHzzO}5`d1rZk-Oc6GhxkU{XU~+f{{$KM zlT+om>&lv+y{`Pw*!eO{`Iv<+&#+AS#IOFla_r;*!i89Ss?;uOuPWuKuB&FZrU8Mg zU7>0MBu*DfK%)lW6O0`EpIvl}_l*g|Yy7K>-%5-}6(a4l9YO%wugtBm&3XEib*U{#+CnZJe*j<>_*h}hg~XPzB0 zaf3I_1z7N}HaWFaM~6)g0>m|N+r|dQ{xGZn+Nym@5ShKI?IKV|FoLNFR4E!`c*mR# z)7zW7%y$2iPX~-Y`_v~(H%2z+dygFH-1t&^hiu8(pp`f-$GCNhWxU%q8c zS<9xQmHp3PVEnVP6wFU>i}~}pRd0rLs>b`^t*4RzGY!v14lN_=s-f315Vi@Z6h3!| zP}do>xZYl0*W7yxGZ^3nJV*IiM!^c6J76vtcf@P$C z{KtP166>LdzeJ;`a_8!-RgEm%mPU^+8O|PgdA$;T+3G5wYH@-p)?v~h*w52nxa1Uvut-68_LM& zOgT3)SY{@s$^f9MAgeZmQ`4u)4P^A#2j5%PjxhVcUV`31>_`x|L#mkt>DQ@bhEULS zADZXf`Qv57FhY2EJf)ZU19*CRE~+E&(GM4ziV`kf@^;y0q9_ck2e;>|#da^#okjT6AdYO?;WZlWl9q9C&c)t&zJaTp0aorUW)Tz8XSsHi2?} z137_`lgy7=p?a@`5oOmEIHvySAAO>H?aBRR3aj{$(`Q4{SHdG=iwJ#wF_(>RnoQS8 z6sE1jQJ+`CYpaDxWF4U|@aPEp4n3Yf@=*EG(J!(bb)~%euA%bC!z*P?-_9~Lx(x=I zq-?U-L4G!X#N}QAHKxieJiZJUbwzwrSK(0gffs-wlFYOFuItMr>+xMVxPH@mjHFk= zd-TF?SYpwO*6e9)D=sy47ou9-T%2Vv%bNW2WeNkT+)}lHV16?v<0{<6p*~-VM~DWrGaAhGoL+lys_=DcsSub7c+FY-jsVmorGIxdFCkgA1(Na(A&e;xk|T42!f% zxoQ2bGKdE8>J974M-LuhY4Z{_yaazU(1AgnzLnY4JaQNUnA)Y=6olC%VW;51GXQkH zYxgkN@VwRbYHBUB$En)o=~vMy>A*s@$&x%PV6+eP zaPk)C0n|{(J^_tLvVOYwq;kZ`)Cvrj9@Z6k%UBZaGTL@;)7gkkRxj&TiPc2O@JYaxXsTZo4mIBDbRAiK7chSd8xM1UJ|21aQHbYJ zW)ilSQ|I3W0bPYRi)Es(X}aG?)HZ0fV7!EfR$W~ZH|h>QNwm9b7`Z+h%y`YV@pALc zHcCCI+*9G}o z_x{Y22C8`R((;RkCj14@#2Y5PV`ew)tU(Yc05)RtIfnokV6oN!_la|-F(S_+2rx=E zdin_`SIl+Q1hhqwX;A+t2KfB(%MUbP1wX+m3b8kFuu6N-f|3aE3pWt z4bXMeltRTx6EIB3&VZ!QmgLFU&8WzLe?F@BHo`_{N! ztX7LfNE{S0GW1r4aGyc5n=6~QZYjHVy@Gl9AxQ872b??}TZEV9IcoqWUtYSvwq;a# zi2T@rLrrIAEsLl1&}_uv{;jI}0Nioeuwg^F;f6hB*UoFp&TFrY86@paGuWHXpFhhk z&nfH!tU5f)tbsxRK9)&k)^*)$%e9-gmY>=CJ>_TqC6{Uy*~fv-_Is zSZdCcFPiSmJO_QK$!HMKp=FG*cEcsmXJBrPp3Z*GQ9qdTqx=~WW#Nr~z06Iai#+@% zcxfld8u`&5`5WcFH@_*2n+=agk{k}e_^@0{BOpTEJY*z0W`ctcgPFsK;d^*`N!P%z zt2z1W zL-~54_IkQH?dY>a?O5H#N5dm9^1?QjB5$MJF_b~$U;5I+<&j4oGb-)7SdJY#zN+p5 zVgonpJ&myvw{@(m^&co>7ZgT+Lg zd7#}won0kAh7deIKXbHPbqsnR7%lyCXkjYCEG$ZASzc96?6{|_-?m)#fB4VKh0~9= z$`wNbxP6*MO0zxJ(oTn)bY!&aEeMej-XG$3ApGa<|0AYcM51au;T@M$>E%m#5f>~O zU4J~QVas%KVOaE=6@C{m?)lfmq}?b(!>WQ&!?zQUwA|fO(DlLJeP5hxB=gJg=H>1^ z7t(k_j48sX7l}Kdg)ksC;R=XPeBzH-@Zt$GbERwJJ!Mnx!SbokKSv#2Ea%T0FxT%& zCtY^PjmzSgv?~Em(*xd8Hf&g5UiZ3t%RAojj)*_TxY0`8xpOCVc7eU?hr-|W3s(5Q z{kQ+NeB>h^c}c{7?>)DbCmwyaVSGtc<5)G4wO0*RLsn$;BA3eSM$->uzP{@&5&o43Es{nwl4yP2waeJQt&ml1@>%yCrsqo%?jTO8tUEq$Y#ut%`Y z{uB*Ar@iJ$RP9f}ZHB+N`v>p&d*$v|y%Nq&^*)3ULo8MK7?`(ubr_vaH&^({hQzNt zDiiBS7B1Mdd8s$z$B96{_x|5ztpQU7TI)3qIEL>`TY(0&3K{$wpmQv8`M~>stDHK0 zf!)(^B-dWMqrCAAZ!UlTCx5bR=KzX6 z_F+5b{CZC2*BuAXcfRwTXaY~9$0gTyz2n~U%u@%-!J`V6a+6AO zmzGWca9G{TMd!%V-g2e7sL|VZh`Zd#&z{QV$82G3QNqvj{Nk?WY0MOtooROQ@Py1JLf4kj2K zesp1*y|<@w8P6F6*;GE3Q(HrYc&-|RT7ZO^9e1zRSzl6mw3{5y0ODP>LJgsz=LD|M z;P=QQ``P4jK30~M6&jbtW{yR=d-jy>GxuYN87|kq@!oQl!!5tW0T5jjIv49m##eTC zzv>1o;U3YIXA~M!3#T74`CC8sQ{}*ON6Rn#!oMibJ*OdsD&M}9bp_L?rH+T&m0F3i zeb2D#uDh=M&imevsykAC?&p54{Pu5yJ1+Tdl-_(h>(IB9d+vEPb<){d1H!t~6U(_sa#M{pVO-g|Chnek*fcPdr+PyX5cO?rujULPb<^-ph^ zU)SlEA(vdc)|HXH?WQ+X6mG4RNTHA@GqVuwOF9mM z4c98q4qn&tlOFwD&rd)Kfj!N0ZD(E^*V#Vf~TxQgL}0GrlBIH=(S4Z=f9ESPfvm8VxT4ahhF$h9vE;lR5fb)i~RWa6E1 zqL=}2iA50~J+Q0vPCm?PyQx@y3!cgVgyb`v8PC4_u4%U3A~Dx&E^oYd8`DH=>|nLl z0@mTj_dg#CgOqCi)IQFz3Q+gNlLukGy-@;j^v9aP(zDo8e&aX(BZifk@`_jN#Ne|t zmUjNb_x{ruYyZoC{lAxEN0PDL^{#u$v(HR0J+(HN`1|g=uQhk7#dQ1j?XipR=YRg^ zQ46n*ihbaL2VnM}D6f0%Ys0F#VLeA@6F0wgAE?D8_9Gy1tuKYA9Djmiz^$)IwtiYg zAOHBr$}>-Yt(@Y_fZ^^7Wf!XFG%M7ecn4ZVP*(fnmm0d!e9F3OGJGIKlUC#32hdo-J}Fo%c1MGda6#M zy^=5>!4+JV>k5H?A>JAWKl9noLW}SM?(TiJQP0m7<@5A3hD`)=Epyh5@epP$C-c9O zpS1fJKib#nS%w7Jjy_{Be1U7%v_W^j>Qx*W?^%isH&XS|Q_xtGR$5Eu?*t~b!Lz}% zNsGb3JSHTrFr}+LO+U!0NnIB^SG)exul&EJ4j1Wz^_BX*As9#?KC1QkEj*)U5)EB{ z^e2y(`~Ub@*|PPK$fTYo7G1Y!I~27>fjSDgo+^z>cM&70%gyeUt`fo7bC zj?irvfx_K9e-FQ}@VlR%PNATm&h{{@#@Day zHg6uM!?~yY&=3C2*r?VH@hJ%~jC&GZNP~tb35}_HDU+b=h>U{9p3-17sOHUZ?RxX< zXJHao&D60vxvGmH#OM88~~JNE<|Fc!(=Df2_DdKzL%U%rA3GPSZ`o? z=d%Z%E)!>2E!BT-Ilr^NETb(fPHriiSe>xRlZHqMtHVkaslE(eV@>3j zdvkq3RrjR?UB3UdcMc(Nc9dK0-WNu+V}}oN;K}y#(LeZTIeK{YkSo(&?s@=Ef8PqX z7a;Te>}hqYd(!m##;+Bc?YzutOf@&ik!8e30J@oao}ivPnOT1u(Q$rtHQaWuc-FAI zZ>9r!EsX!JcfOsY-~B9j_SdUp`6h$IiD=2e*F0CJHtuWsH@>{Lzp(+#j`+r?fU zwpO`Sw-=${DLeLFaXL&$6AFcpaQUb|59G*a57r#2ez-$-4!Lnx^i#*V{E-dh+o*B+&KRsW%XC5gx^&q6qG3_Bkvrb|6u3cgHTwoo& zr5x;etn@tV4k;#voDM@BjNOA&y3EEc4k1}&X2kR8JdDJRNiut|K5-WLxmO1TAlASW z5X@hdMYjUC_rDVc$JbS3a6HpdPEWBuoi*jQj845rutOu%+nPaeVZ#$gl!+TVMk3GM z7al8*vW$3o`T()_@EhUx^<3@aI^%5QMTXDz?+p3kBafFK{o%jH_E=WcAq-!A&%JSK zum`Mud!2adrrD!^6MOWxl}$VEDJP$NPw8H_ogJY6tekot?Ta>4NY?_88SGy z#%uGK8#%gy001BWNklu-HY|B= zq>gz;z6qg92_;YZn+p*H98tXZ<&zl$AilW8UCDXsRC{5H(@=N4_EqKLkQzXKb@d`k zkHdmV{^q6h^CKJzBZmyip(wEVk*{$sYGmnhtE7sHTW`5Tu<>CN3UdMFx@%^t^q%E( zSeSE#wF`@jPr|SZ^xVyn+A=o~wKw>=EJ=O=f#6o;W%pj=7ri?1mtZu93-Q4W7P)Bj zT)u!h2Y}e0Zyv-0B#y&0!N+~}4zYUY`{<)&{r2j2$M4}Y%1%5&hPkWh^!6s9b5<|D zF6c}bgB}FIC_|ZaNP>_I9^5GeWB@{P?N!^$F3Nb|fIW@lP{(-V{HnXO2fCC{ji&)i z7Zsk5aa}1-KKX3q`MAiTmIl<3=v(lZb<($HciFc0 z``N1dPpD^3FaH*`;}0`rgiOGfeMUfinHuzW@8<({Qd-jDVluv-b4{xmWpP8pBuK9FU;}G#~@(TvNTH=tI?L(*|jniJ+xzSnK`x{ z19RDD1%OP8+HP1fIbY79M@`0;{c>7V=2s(FatMIsbTQJV08o&jf3uQ`W31_cx-R60 zK`&l}S%H6n*^5yQ!(74O8Pw_(e_hx~e1>_$k1W|i5n*Z&97+}q=J1Csh;L7uVnL%Bi+x<}CT zpmugWr=#laAXxm$;Vjn%yn)fH%J94XefcbceDc6=z6cU8E%>%QyBIPy^{5PHHl{gs zJcb3HQCLRzA#b9>TR^ofxmHB?{crg`4u!obaMTXfV#25cn96^dt#LluIq5V8c8=v0 z0xxvQpfY&dwE?Ek5LQ5j*X3{EHytni3ki9N^gMR<_%TD!KutlAJ+o>=2nmk4QkAdG z)<)p+Pp$=+)_}qSnIoK%qZ=FhGFpM3(`L#Ko50>1ZwQz&{^rfwF}_`0cCyv7d)+c; z%JcEe$?3A5aWQ){NsSTeB0spsf;!a^y)=xgn(k+rLS2vj3d#5ifhCz8rg1x@r=sYW zmEwR$tv%x|aSD&;6(}S&Y}^oE1{xU14JYeS-Lz$xug8B`!BN@NVcJi+IG2F(BK(&4 z2*m_n)j8vj4~VLfOmHBCJ(Zf%Xt|r`2czHGv1LQKX4js=N!h5Kxpu;Z_jFRbOz8~lJFh9rA z3Z~ESQ*(Gdzn>_ZZho|!{_3ZBcK!a}Ew}8Qj0H}&-}*}QtR>DlJYIHPcVk(<@xgMA zGF{1~aWF%NHoddl^yZ%{hrjrLloL;V_ytJ@IEt&EqqsKid|kQyb^i#BF&STa3z{Q^ zbZlKzSC@)NpIv=J<;FMtoAQO<{6blo-Cv`;?E77u)-%cBAI~1;+(B9=reCwlc}ED~ z=Xs7huz4*nA1!0s9d@OA`}e=K?7iux)|ZiDcn$K>70uIf!E)kEK<#9WgIC1OUSA)7 zIzwt`cr=#`&m5}1@zSNxK)X=e6@ey5gT9?x*RTHa{}WzHh7b}>LOs<0#4e;ztJWJd zi6EnV?BgFVzyIO)2O&2}zscJ!(%;BWV!S}Vw5?p+cOBEJYs>t}g|hX)8SpuTq3{vH zXAl-vti5b*MYXqAt+ueCN>K#%JeNVXiYlyz2S!jG^Q=9s{R5oX;|ztn*z8fA5VdjY zl#mv8X9(Q;FnOLm=By`o-TMea=xa|rh0uY4&_bLRR4Z^6A?uN^>nL2XCG6g{lkW+B z0j>N#%5f{p_K(2(p9JToce(d(V?6$K-qU%4kpimI$w;r-cVh?v!{Qu5Y$`ciVwPn9 z`w8XiLlC%6-x-&FG!XY?eC?LI$_6x~M}OfL0;f}_P6PTLehP%tO4728)CsqZDI#nm zD>=O5WIj~?-zoe4`t#-QyzY^5;`__NhyF(d;$*q=t^XS<=(d)fyWd!rhc>YFmUd`}u`<8p z=d;|eq8iVZ#)2$t zTn-6F$JdpA@RRRF;BG4G__~m&$nYA!MO7Cndg~25LsA(+jCgKhhID8OV=%ne6xTOg zg_v)vuN~HQCfI(`>No$2gQ`XCfFCy%-6K_l_-Qa=*v-C343oV2@b;sk=B zx4iDX{~$gL`oR64EHg{Ype$YhH>PXRt)D;G3_$|&^DuuE2KpbQ+&@+x{%BV@^4aOK z_3Dk~v-gje554c+vSr`ya{Y~)*t+;0;-$vIT3G}LhTz-ZTwZ(g^W`7@!}pe-{aB?-%l6(-(6d;9&`8*{f2IDV&aV|H=Gy(N#UCAOu091q5t_!7!&( z`MpIqAc=DoTESlp0_~pJ7zl_~FH5Y}GXafyA6s>0Fggxd&ob*WYQjh#bK%{wP<0_t z$fLd!p3KiM9{{1Bu9cN2XWZe&E5+mtXz0 zUurR0EAPJq4_9K^)&40(RC?X?U8j+FKN#~Ds7@WtZs^7+sGY}tP6z2%PA z50-!bGY7+Pc<>+-XqzQ^^p2_CvT42c1++^D_<=8vl;8af)1uf#dU`n60uro*FX_cp zm&6QRUVtmOr3S!#cJf%P1vz}^T=~ie*OcC|e^QnwJ|uGC#OlzOd)NlAkGd@4mXc_wT=#4Pje(vvUlNG3W};a2cx8wf#~}PiD7_?U3|~ zR5f{%RgGtuLE7=ckMI;o@jzfrnBnrfL*HIAjjS4gN(>9)CWG)&^cW@5I~7so3k3$q zjf~j`2iV&@edTc=slO{-XD{B_p;!M-2JU>Wg{krCdu7i09^sd&12wSo&O)J=>Z%~} zXzatE_!MfZLxWdw5QeiN*d-YT^kY-;U`cH&G2KeSiI;Nje&|uio?J=|_$Pnaxcf zjZSUi)YGiZKZ;?N)0ZiiLpotN>!Xvn*D?GUWVkcOFl~hcO2)_eK45vSER64f%O_%r zRbA^WM~UlX{+xCVT|WHm)LDOqYH#bM)33S=%QzYV)2V^oUrwxh1-&%GHPnw_HW5rl z;qEhz@};8`4QD0?HQ^D+`5N$D6Wqk-MwHH2P1KC%3m~ITyz>&-n~X#Oi@ZaLw*qTm zK+L!b->(1kAOF3J1YWP9DG-pE2&lXK@yGw9e2inhPJbO^Pyqf*e7T$e zusB!pe_2J{@m8ji|C}V1z+P7Bh18vW}6>0kb42_yrXUWdqHdi;g;xnf^H5@tZ>2A?9r217Rg@t~ODgBiA`E%@o zB;NYtyR42Nb>DJJQ_*j=tEcp|d|`fF?263C9RG(MsJkym_xx0u z-f;+x?NjBpomZEC`m;Y1b!l;|zvPR`A9_O(4 z*Oi^yx1pM^!y7Y6<&O0g(2EC$_>ITKIc8~?I<;3<0i9bs&Md{D za`9`-31x<#V4uXwaT$e+`k%-%a6PSOXbC@zf-R9D3S6=E;1Bgd7 zg6RKOb3Z|$0eGQt-6&n#c$7m#r}zKgn5YIg7m+oc^G;2xQo{tIi_5hw$rnvkMp*Q( zM#$=pYL3~sIL^IEY@kwdm6IQJXbH9-PwB(e2lH;Z8fshVnPIkBTL|P3pd#FB6cMf4 z4Qu37Dae$tYPjfv@tKxR7a3GA-7$~LG#^EF3dPOuCzq(bcFZB$$&?x!{ z0Z%)20B|a%W;50FGy-GCPobGTiAUOJra44ZH#%Sr{H&*7Abo4^Um2_kGerK%7)Goz z7DFl8S4&Dn&?T1^@bGWXE%*vhc`#s8|3sB{qg`kZ>rv3Tv8Y;90!$+o#E)+os|u=$ zG4p~0N|R^BtD8s~1nWq~SfMRcFe1@pZf~)UopZ=f7d!_=ndv~aTCu=Fs#d@<4Zp@yo;x_7v=6q?hXnR((3CfCn^mWp3}!!r^Xr>!!yg@8=2 z-NU^nCy$<{mmlEJPc~~2eGbN7-Ntw0`ykB!e+umd&bYSmYxq34npDo5$q4X947x3- zj8vUHaQacLQTK6%Uofxyq|0ljsA)U>GNgS~_gL*J^Xn`BnwOQHc{cCq8SSpUFW6r0 z4yH!p`{$Uiq?wHFD(g3ol_PxI^wX4K(qHs35u2 z$uE`v;?6~$NoSa8+UWwO^&jwp1zT~OhxUy)!jD5i;}{_URaj=y`5F-0Y3=#r5WyM; z<_Dcf6XO|r|3J=kxY%qIc8!ZMJophRO|@`p3iXrwmmhgFJ_ok{>1Tn<_t!X5`aIJ5 zuY!|M-udQxg4x5`)f8%pai_iAB0Z+dl3lWl1`I(jlzVR*M}=;e-m1I*@s349?VxY$ ziG?rI&mJg`x|xU%UzEo`@v`BZ@euu&;F3@BFbx^{a?Zm{T&pn5r+q!(1Sx=EU(ox=$=G`*^C)zI|`t)oT` zP>lv4%wPGXU%*S$y=4)aw@9+;xn|nLOT!`R3YE}0#Ux;vv_ZwVHC;fY(}U3+D{8@Y zxvWMY$BmOSFh5`NJ$md|NOw@?xW#ZG#WRXK7dLGo< zGZS^0Y$+@%D|f&89#r7DP;)Cb4xDIjeyF!hkwu3e4A0n9h9}~H3wiobaoK3)UGuG* zqwLxG`iAp!R4{8VTMR$+iS`c5n#VVOuMGJe7Pg~rDCLc;o&V&sbjz-aJbc9>ylh*_ z6(N1=1GcgF=YRO`|1qEU-UY=c-^f3)*$3K}`t@(RM`$32QnEs;gKYSyjooTNPcGywkv6WQ%jH2?^xg)h}HGI3g$%@KuIMd@`ukUah+_ z(nZx)BWSzOg_;5Y{!MR>UQf_j3Wjg2!*E2vQVRf1buzxY*|Lo3FC2wSx)da_uU|aq zA;XAbzAe0o8Au1lz~>0C(|l<2@h_IEcimVX;X@^FeEl04mRu-XuiDH!^%PTLYXc8A zLsjSWmHA#&xPg$C?nQd{IcyknFyqWDU%Q#|>&29j-vxU2*m)@9qsB+q-T>=$S?-Xl z^BB**WDesgb>pG&R=>V z5XJaQcUixlH5S)gQ-12F{!#M$hRz>6Hlv5ac($vI)5P=JUz|7rcqbiON4I$ai!sNqjM0<92-4T%q9DTII_ zAs||HAj}3W(o9ax#O9sFrSoNkLrITtj{AnSBjtwcuM1Tzq$M!KCx$SjRovyIrvl1K z@@TKQ=#eL<&cTdRsO5Z&+g*T+TI~R`dqxoOu29hR_YU&OY8K#NIS&IE@hp!gvMzC^ z{Os%m2ZFF1R|0|A7w0DV8V|>RxoQvLpf+$3BbTfBgdqX=$!iZ_nPpq`Me0>;;Qb%_ zQsnnvFW>GO*im|R50{_$p|`Q8VQZXKIqW)NoXf6N{pl~-#3vPL-0WC7FJmP&N3ys? zM-z1Hs{o|-D>=jJTz?0ln?7a{YP#f$DlqhwFRDwSkOCmS3>4~0N8S=1DZ%u)j&y~mV=FhC&0*5V;n zADFOfcIZU91^Kn`0~a zAcETAWFBAVF?Kzn)%7aG5#HTwZmM(sYIf0KL3=^2L48dp7Imv{hGbRO4oxa0gdmwZ zV!>Zs*o~@7UTfaD1$h#f@oTOE!LQNU6$Gu1gC^6PvL?-p%s?M|uV7kL`=*!A<)1K= z%uIMNy|LSPL=lCNE)g+;>a9Pzw+ITU6e5d4Q@9vvpb`%f$AdMxm~x7x$QsK)-0!A4 z!(dg$I0z_?$CQNn$;!_>cd%^Sw4t0hb((qNwPhVsPsyxux{VRhV6H4*Xcy$^IS0#V zFf$zdIWu)O7(hHfWmkB#i5U|2@44i(X^&@UaEwC}I03u87Er8a3h zOC+h?dCVPqFFYL^q0sBz3^!>-M@2(J8+m>}s_W4iM_oe2&}IAGXW0`zqZ85D9|jHk z60->vhJki)`EGFK3H{}DogGPU%tZ@HPz^K*&K}3iG<+>H-j8V)RC^D`FwI4N(VJa{ z;RCz~00<>0?gG!PikW#;S)zbu82J;&eEALb4o3kLI)m&+i}M1hG?ybctZNxaMq1)4 zfE=g$Y|nW7^vQDg*wJ$B&TVCum0p7bd}bR7u4J~6Vtqm)Y7#1lI-p|R(yE7>TwJbd z{`R`DxgvV{%+{;IAOwK^N`6l0?d9j895$+O8{o!{>dk`~IhX_ip;PDx)&<@P#Im^J%!qaK*#>hT0GFJZdONY(k zzq*%L0~(0eZQ59dXuCW1?1+t0o>c7&%rJsoReuQK?osvqESRzSV6PWfIgY=@m%dh?TjRGQQrvl|Jox~q>F1G~f*6?J3;4i&0t)4YP zbAJI)@c+H{zb|G1l8+!Wn84mQPP2jGW6id`jm)pKQ{N20DN_=(C;`kLfU49|0!Ggs zfd~V8Xnx+~6UEjb|Dg%gINXB?+$140pbxu(OsDkB{5$g|prc@FDyR8E?`J>v=WG?7 zDzotO?rU#E6X47^H>O~hh(kgdddSq4Q@nFX;@rG@#^;04Vn%|n0i?GwQbWx=rwF@= zWN}`NfN?neuziA?Oa`##Ytc6!59zRxMJ?(UIJ!_5$v03|5K*6MJX!zQVzHhAqOu-a zV@Qy+?AB+jp5u8gh7}WhtKb|TqdbkKf{~CJiBJuYN!kzML!S&ss?o3ytz#*)iNxo> z^y}!zaJlv78-f<@e&wC9sxjMbrHrspDVR2Tepg7M<9gTp3t@DM2l^zSwZx2!fc$+QAsdN3<;>hJ^dV7#V{W(*%5|34U~1GN@1gl<&tR@DC_z2=gQ>S)3JSa zYVtf|I8Mt!jh`UjHSDmnSXPz$+?OavH^UMIBSnkGP2*-F=9tj(>9m1Z$8P?xVnUe6 z%)ANHGu*476h5_!Hj+XQu^aVkMbtO~aOmKW$f1Zm2v|WN4i!ucG4b<#j&{6&fcSy? z_LU158@sytIM(_pzQBaGKxTg1an{dQcRH5{eAA-{foLRK&o8wd9&lo2e#iKIr=|hh z1zNjiG!~>;m)GsyRrcL>6WS66S1Q=Hh!vUCwW|+%F}{v6badfSsOi*~bnNcR%E+Ms z;9ozW>*`m}nhbiTofPg48{)eX?1AtQRe4I=S!&qX$&!HF?fmQ@)h0c>MM2={PE19E zk6XEe4w??5NfPM{Xo(iWLnwp~h0%2|zpv7mns#q%ybVD`uTKx@!W`B9=y^>@PPy2i zL6WMf#OQFqN{M%Bkn;26f?tkvU=;o260t|kkpY7VHKYT7md}Uv(BsQA4mm8S6|11I zSh7lNOBI(GE@Zl_Xe*hX6zOOqdC2F1=bkIuuj((SS?4~yzPsFb!=Ct3PmG`BKH|!( zA?(Lko`cX4DqRG4Rd~0nz04{7x`>94UF^Haml%#wE~V0 zM@G;N_+;6_+*ldqNHKemzOVzNSyQIA2pVi+2<6ld9@Z)~6L-UXig)5kmgyB@!N7pw zU7}mSAT=MVKO%tXM)&@U6EqZ!tQ=WV2&_yX&~hsB)py=T`4Pl?z-69YbMp)(9{BPT zL6EkiznQ`L{I0zjeWNZ`|H1qaZ)~_9LKtwUDmBLXpLGq?Z=VntV`G|oQmo8dZ+RV? zoA^T1n&CK$-&3gdmu?oFsX~xDUG1I&d*1=ne%ERw@h0Q9ReD<}TP+UP1ct`y(9EFk zDinfd9s5Je!Zf|7DcgW*3(}*#uzVLr^hPMid&{%A!cm%Dz@{+I3{al+=tj z;LJz5G2iqs3q6{95Ksn%F`0MtYL;t51FHD(g&Q*WGhHHdF;sn-gqRRnrxUQuQ! zN>MeEaeJM=wucXC1wzF4*-vL#(SGsQIdg?eR3a4z=KO|*l6tNW9 z$_EG01CbV~AMP1O5K2lApZ$JP!Shp`A*X5WLLg!6;Uu#yd5_Ud|v`?p<-iVAQRQBz?;d(GWC->bt7C9b0O}ITn{+ zB-MH9)i#8B8Oz(eGAXM(Wiryml%PCQS`!7yPk z9ox{rQSt`|n12mKIX1qP10?w*IYMv^j3>d0dw)MW+^li@f*1_{)57wujA2_U3jVp_+5Cl=dR0>3FdVeNO!xm4ZtA2qf@vAV2{#inHh#)iY zXC*8k5qrSVaeAm(B%GI;8=({x15gX>-{{~Xr`%4ISMJ-w!k;yayBu=xoj!VnNsje) zG0!w=?=)k6Rb-v#-j&`pG&=YqTp*QbD2;r^akbw(Q-OhXyOlLkI!|8mIOktftPT zaO>&C*1Eb3!(WD&Bb8R z+tQX_d1^lkX|5|j_TT*7SfusL{wK=4ulb(x=)<2*f~D+^dDpGuOaf|l2qT?#m^IOk zbXN3)GMUD)H6NRJgXWgYCo`xo!E_lF>0-UuaxE@Db{XT{Km@5Pwie!!^i)XV*5CKPR(>bvu zCtg|=5|sj4ow2PnlF8pOhfm>iS_26z^Hbret_wRQqE8_NEqjMkO%jkQNT-W;>Y#^3 zFazuXw^AJ|56Zkwe?b`iG-OnWL=bM%C<=mxlRPPs8u(9%EWZaR#N z(e1*1c>Kz;#gDL7`kzTanb;^RO@7Q~lqpUE~V1Lu215*KZQS|}FyL!`R-t7ov zuyl$WMRrUVbN6VljhmSwgYjfIg{lXS_VdZ|K{N`d$o$z>L@sAepCCO)$Fq=Xl7nIH zy7{*9`Zv6}Ofzgc#>Zh$eoHssWEjNQb^&8-KXoub`q6b8VoR_L)rYM@qVDd3;e+wA zhgDT~EWCtZ@$5qxTZT|YZ_#?a6TL8pHp2HWBS2iE z?imvB99WS?1{TlIkSTKvZ!lPSu+-^*!rEaA{ix?!%;)CNoOrJi$uYFYoyUeMm=U@U8nmOhh>(b(k$35WHkE{GCn5Gp z<91LD4eA$+A7T_?klL=rHLdYw^!Pz9s@gKU+CVVA6-!;kEKkHUCCn*J3nPH&lkm7T zegyEJbf0);hIoUw8lUSu7~n-b{en-yFB|}c>D3{8FList{UoS<{j7NzVwhd+auF>m zP-N1Dfa4KEFg@*((^*g-PG;(+GFK=-SgXOx#-^aRM&nY+%iy4kDLe{3kI~9yw)RHTOy6Cz zV=bR4?`N|H#BJ{~#797=(5zHc>`>E~2ZhlcYt6gphu-tt*;xo~GG+zl>9OWm2{?~6 zc9El_v^MyKpw*7|V19ioW7Zgz`7iaa;q`uLc)sTc*ND@>{DXWo*iA+Ky)ZBP=Vj#n zz8y5UdkzL6Qkv2+ySoW#&jspbZZZWzsQiHI=RCV1)+#u=(Z~14){U&AebN>Y46kt4 z(U1y8+ts_wt$IJpw4uj>*@_+xJL?z}h8SQG%@DH;T^K-@Ni&BrwGV>q;aiTI2DX)T z8^&WJ)p?E$|H7BPSU&s*AH)uF6}{<>a-QMZZ5#Inz0I+IV`-kRF*P6eaCPJeJ0zuF z86P6R1~3{u{VXIy=Xh}S>a|!V6PmjT%ikx zq4AAM2+&T+I&k`LQ2ThWX<6nk$>R#aDI}c1XwBF{Mm4(xkB6Iu|?rJFxH5oG4_yS3_ zNA<~Fc`-G?-LYF}07Ov@^Lu!fDNq3e)kCEtUcsQj{Z;~D?rh7Id%Q=4z`H>N4@(dM z)n7dRf6CtU*Ow$Y&pUVTdwZD~j)o(RVbVj3lqd>@3CM;%`TzDOeIsB%hG7FV2-~nI zi5hV{Yj@v$_4B-ML{**BO#{w*PgPcCY#A9DxmNv*=aR$>b1*oEpjp7f+j;c$1J zXY`-*%>dt)dqr>kkxKjY=>-%SMn2(X8-7ySU$b?^)I!@My@Tgl8n#1!=Z2o3bCKuu zTVC?v)X-~ayOsCy6%7fxFJ0lszU)zV)j#uLdt(|=;?qdabpS=bI*WA%rr&MwSGdNP zkLwT*ZSe4a`We4PNQ1ZWKEvtQb3buZ!!Z9D4$hq%e+5XLe#3~x_c*@(=C?QB{Q6fn zUo(AqLBlgReev&yKl+yWHNRHOTFNtaDV{MpavEn3?=YGbG8_pDrc9yU1JmuoZ|U`) zGI#&>=Ih`7KlJW@K;84A606!zJO#;c{C9uq7x zW_fLV`yD*&RBPq1C1mp7m_3wU}(b<>J*iixBKG0uU zPTBb!Sr#y{!K_Q!b1nf4b$joeIl-*bNXN{y>vVG5(}1-7_iVG#89Z=}iJzEHPnpKZ z{yuuS=Z_1Zr)SS9WEvCYw^8^nBPDGbn%+)Z9-H=N12W_`j)HV~+4x;(->vcw`Fb)P zFlovfx`Qm4p~{^ySPn<@d_W~|g3okOaT2(9icQ<*j)H?{=A+@ zeCJXA4xc@*-++Jg$gK-kTNws2On22ZqXOotdQ&Tk(lDCf*IsyzzzVRDNW?|r8%8%4 zKztVybw>Uo?JFAn*S}}Kp5LbPb7HS}E%yzLx~t~2M?AE{(fjO1y#75uX7Ou|$uh5Z zLB)eT4(s#n`dJTn!}Q=SL*zFY+I!L{R(>Ves&%E?;W9r+oIqA~1e`@~12yCO;&l*Y z9*&_~hB^q^@~jPcjo& z4>O9$$;bDcZ=lZYW&9?B$Bw^w{dJ}tjxM@8g1|Vem$R_wltZWV^h=J|GN*s~rC%cc z^5#$f*?-RuYyQWZ|Mma;4>!O5<*)b^qMil#^Z)u!AmB$;Z20uZl;sJBv7Yi_luR?g zwak{w-t8Gq?qZRBE9fZb4`cgIGj^0|L)=C1 zc7zVkQ8)yjGI~y<&p20dFkytU=Iz~HyMa4sx1EAHHGbQiqmPU{xcjxJT|8AEy9-zR zQtY*(@yHfh%7FdiEIek{e;T9LeKYalQrz>8%VVL7Ey0$V~-a^dHz$Y4vp?5 z9Y?{?>D>by0XVvD8(F=s^jxfQxWPC9TK3>stT7$#Iv<&&Vlb?DPXw2rb5f7(e)IL$ zaQ&Fx{u|!L`yD6so`3s$rnU@o@y74j=E^=RJ(0%q<1cU~PldmI_2tcf;;g(q zedhKyc80M2thKKzyTxZ~+@&FZk=G&i0~%Zo0DblN=JN-i#}Ku2cLegfFS=d7mHlRNug*-T=t206DBquk zKSrtkaOu5!(BQS~SI=Ev`RwLz|NZZ7e!@D$zyH5&{9o|1@IT5|dcXeFes0V&6Sx+p zI!?dr?VmFL_BRLo%(r%=owy+U4m*0tRD;$}XVDRBY&G?iq8}G+Eko%$c{&YiHTH_| z4?p2_u|>h|KX82@nYrr#(?QX>J$cNRn{Y_J8(=%S=P_VgAPC}$XG1fdUtBlAu&vOZ zok#7{X-KA}w69MdzvuaAf2CA{Ay9b}9n(vk%Lin5$U(1+7)*5Jpln2K=aFgTcqjv@ zx^QDda)|%_u?G^UY#N{o4Do`xQwT!|*&(KPASo39%&w$bQvU&!9^8h$4#6#y%x5&r z{Bg=Dg>|&SA`#wQFX*rD+Gtw9-d>dP;_5ATpQZ_okf7kCW$O*t&{)$6Wczvl1@ zqTl@f7k`Ztpd1<~fvIH|&TM4FfAl#&RrR=U>iIb?7uBAyZtz)pm}m5^I3e_4@A3r) zXR=E{u~XJ@VD1#yyrLeNXaKC_X8eNUQ;AgaH;hsr|W`D$Cp8vp9F3#E=4P^T* zvgwq+{SDixEEsr=aj3~D4ATtOd%6vV!@<6;L3>=gG@k!9 z!fA+LB<%wonICa^kKMhtj!}pm*EU8+$oUA2os{P<5c0A4j6B8jTMke~-@!{rSuozH7lfQ(jKdi4giVdWUC-czrp{N$%Ui!*bY5M(Cw)sMg6*!J&k{`u#;l!J4`RmT}Qt$=l=7ZmB{8%7>4e#>_v z`303Htb=?;qxY@Krz{da`{GA=;p+!Xe_Ye}oP3&5rl#&tWE3?G#hP)2J&vHNNB7n= zC*{brCO;sz?*epJsQPt*)q2zE*XG@aXB9q91j~>x2aD_=bdth2eeF6!Ey6Q5p5@5s zI&^I4aEwiKo-bal33>t=pq);aXj0^rNEaw|64u3IoUglO+N$dw>3nr$o-qi5h>+`z zMM8%MJnZ3t3>0wl57-*gn`(FtOWxSy(3=?Sr8Uerg!x*lI{*(5+{XWa;m1c_T35cb zkL8vg(K$WCZyNn;KC$(jPhLIeRNnLFyv@c6aQ3pPc#Tkz!_kH<<0CLqm|oou>nX5L zzjEP^&sDS7^pwS?FPM8W&R|sGxA!n&yt6&7hVVHn)*S}cbE?=F1b1atX=#iqPM#S) zVHD`Xay$L8!t&~}lIakD>{aFB7{K*8%9RRbcMHk1`}`WOaQm^c)jSZ*;rmK2;X6;NkZcUuU<%dgXX8X_@Z0z3Hw1yBn|HFcLwQ*DQK| z@Z=x3Ha7LWde*UWIF2C`#l!F)@APB3p8Fg<%umN~VFryVmBaTG02lDbW z-#V%^WUJGLXeZ^Gf^X%qCCSvy4YQ2Cd?$oPe3Tb>*G!zNzX8YK(O~P*Bk*<=-AVJ_ zM($dKqhOv&2gBG#AFEqql2{GNSc3Vi(c&ANy0;!;Pc-^&!y>Z|fjmb763})ro#2#8 zJ8Y%^X`-206fBr;Qgp4hDCwhf3=!348>>-!o-K!@cGM)G>1`+ zB%D^fVD8OcoYQI;B%MDE{W-&wFEYS&%hd+2p?={$HXqsWM=3sM zap?)ez|VPBo1V~eQOruun|M@m4s4L_aQ+&yC^B9)QyVgM020~}iwaaH_wm>tk^er! z=`0Lk2v*KV#>T06aD&DqwjG%%1>8GrIy2h9DUiwak@Ch>~gqm%9f|Uit8G{2hY6|tWA#% z+xvTWvCjtQHme5inRKTjpZi`sy7v{?G;~z94Id4LRyx(h&WrLn-2W#({qvi@__LQd z?)Nu;^Ebbz!Qu%}SdK1REE>GMzB3j_c3ETaNVZO3oPW}R)ap>v&2#?z%9G<;?egj8 zj2Llt_w5a75HG%^LDQ={Y|nJmIzdfw_-dX1_WHUqo23rUhGlwQ%T8lz-KtwhW9#OX zxHV@!4c#^n+t>bE53!L((=dGtRJ%wz_}4fOvLI2QBE>#^q%1lQ1W)^P;az*N!`1=B zrWt9*B?GS$+K|!6JXl}0r&~cAwuxl>4rG<%Fwg~Y<6UI2aL5ONECQlX9YA&oP&}#i zbT$}ZZ1|n?`h$vWv9S$jPxOlS*X^aB(7SlbE}zw=Hs4b7?!vgZ^zh~^Vc;gJhL zoCCK9ddko*``--jtQ?L1YaGd1<9!ysJR|-jUXkIeBLRl_&P83c(et~K+@2WkuLYrX zm1e`QccpSh|EX*W-Bo1ffez(pn3n%7jlz?Peml@(wyul7ASbO}eDfRRd`6jh!w`Kq z@6}(Wur;kfF*)T!f<@oC0~mF%7P5@2Jyl zoR;kor}ZAOASquzT=MwsS2v%rbKv>^_xZ-5#^N7uK+^KOqjef3J3jQ7S!_j3A3w56 z%~azdJ3tO+eG^jGoKXNW`KhXtZ$)uiQXAQ0tA5+?zx$W}#mz4q0Y3k`0I}mmS+;?9 zQ4!yia2$AIRG@Q6*u0Ase?m4xhUhH9e&QVz3XsNXx<5;1(9vPhx$eb|!?mw8#L>HT zfR-E3wH<+uUFV$~E0nZ7#+!yoMR*t2s%nQy#Nf?#h5hw$(#--`SZg{m(C~vxnhq?E zN~dJpMvsj-uh)iS$E4Xogh8l>Xz2QA&T^B7wtZ0@HY+0wBq;Ij7ZQhPKGH$dp7aP% z%M?PwD9nE<#zvo&QqpY7cSmi3fa5BYpUgUxZ^A_<%p1 zn!`gEJ?#~IL(G$HIdTi%G(IVzr7qU!?CAaBV_D^4<@!+$PZVR|rvLyT07*naR0)2| zBH#1p{4t&R$l{kh(Jz1TxAdUxS>r)R+pg~GZ#*UU%=s&$na`iHXU~xAEfE z2WuBrl>D4>7(Bu$^Q@2HaPGm+o_K@5n1dqugNfe{+hF6iWfFW@#-8w93`?-VXWZY$ zPnz8?B-n_Hu3blX{|?#cbKi5?@-6e|r__(TCRUF2;@Gdeu?ASV5ZGxXb<9Upbdnz# zF27^zQHS+cfAzPDlc~TPbk(;nP3g1`_8pZ>W8cQV>57=3oJ^taQLeOWXlpa?(Zzin z5~^&V?p7oB(M?X*kz)@khMC&&2J_R=aqyNHnT#j$MuS+o?F7m*NO0Ts@0 z$WMg-D$BD3zA1%8M5w@dfno5((SC9#!res>8_pXVUJ664q`=Vd;EFw|f(esPjQ`YL z&yfSKWzyiCez=GvjC|bx^$jq4e$Sda40VAfyA|}p_MVvo&)n=SLx{8!N}yAVBI7iy z3>Qb}5oz>QnhpawaX@cA(YwG;w#g%+3^w|{D58VYL`-vDrGeMTefH?4-xheo=;rNf zMkF*ItBB%6M?e1YAKpBF{msn}e)vdbxH!t}hqX%*xGt&9+o~c>nR5lh1HF8>?#`-Zsn4oc?F`-`nGS;!e*F_`wxEsqT&p z4ga-Uu#7r>!E4z2Lz_SQ*;78@_N#n4M18e6h5fC|=9zEYZOjMg>j`gEK4kmSA^ZbI zepqU3Q%9@Os*l<@ibKLUEA(V=p08W5&YNpzGdnqfHvRG(IG#bUkxH-TA|D=e=ztqG z7W*Z)X9>u6bQ>pZ2d2#<3p%%rp$0(Q z-}g^mQ4R<-UR{lq!JG}ID#ZqB&#Y&6sGS848i>Ms{1*bi`K8=N~M z?F=2oI`YuY?3B_`k><-lI#;K5wz)TSX3F0+xf%M~si`F39F=#(pd8pOSIg~)b;BW> z3Cc3gucc24Ac2Aa0PznLAmxN^_7Z)LVjw>SS_Zk3mR^TOBXd8fpdr{B+r!$3>tseu z`B-3OQUQz(REQrvNW-Ub-^a*JcfK#rHi~;W0p^IpYO)ekM5GIEjEw?jxT`}z55^r) zxK?0QwSik^p2Pt@qmjg&R*2W$J<}V@E+hYtBeLSDHFoFk)F8B7x#tiD>1iy|P=VlB z_Ba&r4ct7GA+b6!w)6PjpgiAl9Q7ez9dh1nJviih#hQb3zh#{x@;X|9pN-J37|S;= z#2|-M4fa5qgh8&E;-TM1!f5#5Je{qyH!r{9(_nrGgtZDCsj`Ed7lSCDJl|8FuG7?^ z`@TUY>@@yw!Xs-0VOX=uemUhqyzC_S=ASLN-za!;^RqwXH`Q3z`FH=_xyN{l@I|5UQPkh01$Q+FKVj`93;SRT8eB* zx!|WW$krTynY&SVaWiMv5rnrrjE6B$qh%t^bpmB@o6WdC_2mzL=s3hZzaR{K;B+K5 z40jqFrMS)L%SI~PM(~n_HRDcy-1pV#IDB_&55j4z2m|d1$D4A%(CDP`>i+BMfu7Sh z?o78~Z_gZgF@z7aDnDSFeDu3(7DQ<1%Gtu(+gQ;S_5s_E9{qL1@s1(6+p&=eJvhCx zA?viAqdP>m46e8An}4bsi-?}BQp8Hb_PqYC-qYR_vVBJ*?Fzn0jcexF;(f?rBziD? z$KU)i?_-19I;>`Z6vGd4$>Hg4m-b=PJlPuy+%@i@||lEO-L zwei~^+u%D2wBchfq-noSv17Yd8NF}JRN@BGLJSz=n`c`}hh#SCoC7i2H17+tdd}NlEnqXlC5bVBTi!=9? zMLx=BvWif47g*kUtQpz}MPpDl=GfDdXAL8ciZkl&DllBv5o8MnW7BXH!Nom;`~H6F z=?!!Fhd6=9^z1sGE?hFYu#y~s0IMD}H657SdmrCArQx&#Q;Ij7_Vb{RUk`o3?+3i1 zr*^;mA@kl3d=t=ei0?^B4K3TC^z44(T=^X#J=x9ybZK}7>a?AB4KT6_Tk$wl^ zBQHYPGyug3I2xlqn3kkYsH?X+4`7r@hhd|)p|?3%@`l~6%R$FkxwrBAhNjN_$3OYu z&Ajg7{{C0r-uw^$`oBr}b&fqyrXf>5TtBBwyC~*}(Q_$1_` z8f_&6PBDn1p478#Gd3wn^-P^>CToky#{jOLQgvskzlfha}Czxq`CEM19K<9 z25&=6!zaEYT;-CtZDA!)P9Kx9LhW6s01&woSDu3P>S;vjH;^{*EKP(4CBjMQxbwRJ zg4&SN&;&;KYf#8;J+3SPe9Q(6|z#>+f!M6*=l>~HW17f%Ae4ugaT3#Hi; z+89-$q^qRfchuofDkBPUlzC)0u+2 zEsJjL=*_7}V60RfiFe^rM(C|P8P%|0>DPMlGilIygVA1(Si2JMa9deg9?Fcvb-U_! zyu_2mP8zavZI460Wpwcphv2^aTMqK%O#jT4EtW%L>tENN&qOhPe&EYlOr;zOQp{%R zWT>4CN-zW)bQdknFC7kLXOWGAgrRrz?(;5y`3sv~ec(-t1EDeIlqL?MGI^+`PUR_E zzrXt}2f_IFulON@-~5iVA)E#Ag&{jer&#Lc0nXFPvR60 z%xy-G%p6GDpfj~TvXDz1=>VPbWvatSKxdF&!m{JiQsSJ;cO8SYv}vj*qa!WK#;y%n zcd5TvlXVFU^4U172W82qOgka16`smyJVx2*%Di`J8ARLg-83_CImauU&P%mo3C#RVyuq2%V_?HawY#9ABFBEd0GDT_mm;$(H zXe!#Cw~bGXipouoY7HP)6`-4$a71_xA8$lp+6uJNsi^BS*gh}Z&h@i+NEw88?w(V5 zz{#zPS`Nvr{L1_IGrr!VWfL?4KqH?DHaLfWfD^EXdi=ppcF}t>e|GrpE6f^a=JgsM z#^~qC-*YD3>4VcK8@xYH_KNTMy?DW^%N(0d!)H*?UGHs(Jkt}qHe}%a$&7~jM58ht zoDRWhq#pFqc?8EH`ZQeNb{_ufPR~w9dpMadp77ES3HIhaR?Pw@WpvjLXXf5aNwrZw?Uv5Wkr`f4#}<1N0eX>7S;lC*B@9jD zNxM8BX#-fBgB~Gc+JlH`OEB5F9PO4YwgFKp5me268)?b|V;YU62=g}h2UJQt9R;~h z++Q=1UwASau)-YLs-WH0JGaT2t`zMt+mLNc`3Q;cK7T+4&Qss;kq3=HNjjXOfwhb& z#Fh&QE59-#kHM&y&V%xl=eVVT5u}io&oa2L?#gx>ydP%t&$SHG)8N7Q@WEAW$`<1& z5A$>s>2Z#^hhLumMc3sqxU zhRDRZX@f!phUdN&M{~7fqaHk%Lz4xR)1>6PK486X2ap{m>bz?)q-#tN3n%P8I3#_* zIT_qhy8qbGiKE0G@Us55(-1LECD0b3CCIeUvl!p3_dldBU5CzTy3sYt3jMLc5jpl~+8*Cqb z!VN@w`GP=vjLu$7%gk^F91(*|exQfA`f|B$}H>~R2<`dQwdvt#k z)cL(VR4SbG{L&9=2abS@`*zz)-v~q>&N-bDp2L}p% z)r7bi*w-UJ3hEHuy?IvEKjhrM+1+XrM>BlNd;R{XX|_6T>^d+0>;+#T!_6xfy8x6q z=`KF%2%qvMo{r9U=QYc4bGDpMoRZURf~L4P5iG^T}uCv`t16u~+1e z&F7&0I(J3~eAB?si@)O|I6EqEYIu*|6mXZuk&lfufp+8bZY^uueog4{Ll!NuZS~}I zAi@LzxyKfdVo-RKl&c`zCo>E0mUZmQsj8iww$RpDJ*86uIkpG*;vemVG*_?ewE!!@-Ffhn6zb1X2Ky^u7;mQ$F80g7~m{A5llY^3!FMcJ#rXXxZ{OpQP#+5q*m z_PpEVWm}<$j7VD z4%r^^;n6o3u<^GTu@#-+Uw|44T_>bsKpO%`sY;Y^AA6Wjx?mN-uD-VKhlW&a!!BiP=liYQb4W0N8O$Y znGOERG2>fgq^fy#Z~y*BdS#sfdi}^wf~&!quaoxRt2tFiThBKV=;`x7 zezaHjP>&9657W3y;3)Am?|Qfv;5!$-ov9OW6yeu?za;KA1>N)5g=M!{Gq3kx6FCpD zKSv0!_`I8X>nPEMSM#Yq>N_Jb>ov|7d%`Ki5vvb$WWSliQDEqp*HKt+`6X;drtYdZ z-SUf7AMLfZ4eZN%-lWs6tYa%&I{Y~WilcMn>YSbtfm_|;IKoK7hMz{|D9B$Mvd-@_ zb$rj9%+Akp#1Y_Ba{!3)3NNp);?a=!q>l-cHkrzmAdb^*pcmFVK}WqIqZ>h_fDRm; z`i~7sKmxC~qu~6d;VdRk$)@3Jm_S6iVU`?ILb(DAlN1I$_=esay(`~Wi-$#VZ~43M(cGN_Mu|2zkM?5FN{@2vJ-ufqfgzCP z+F*I|a*9ew;Y`pNpogI#dT3SK8+qxVB6F7WBKRh>i3?AJ#OW$kE($H3rl7^*HGpkoc%A-=yS&NkLM9O_Nk__HqU`O|Cij6vjo z3atX56HO2o#+7|XM8W6%hqZ$iS{E-{o|dHp8_wI?AZ*yk?Cyk> z{odgs3zHunQ9gSd8<34p2SVJ1F8BN6#3*|f38#^DA=FoO{jt>-^lS{~J@A8qtrh@o}o`(CFg((dpL+msx8wU!B^`6WUBQ1H9rVA{U zz)?aPGe8;YkZi+MfH*ep8h|UvE0mRPuXi{byTLg7`Lc8;PIO_W119>u{K|kc@18o8F#Hn$|^nzYi^gG}Wy((72C0fnzy4 zKW>@L55BN8Rz>bhMT&HmmjXy6joL=&oZSXwgLcTSUD!+4ao*>prLVF`sbiII3|Kut z$NEbnW<)0sDj_>4pVPB-3pJeJcZMxT8rGRiti8JO>1Z5A>$u#KGtKgZh00JT4CVdv z10}u#@r1Pm;T~^iehP5!N$+~O^6*JMbN2EXbsHIsU)kO|SdOwHF&gs@WAv})`Qz`Ht1VUJoA5|tE1O_)$+8d2WJ z>;q09hNv=u2`{s22u@+lS6*>U&p+T?$M8o+7~9IOLDGq}!RNt>B+b`W1@y%wjkX2| zq6SR^ZOQ5gLc+#xL}x%q*-cQkk#iZ7MEVM0+1;-om$JL4%?3z)8RUzIvTEel&b$PGRDEuz+H=oqlK5Yfl||$&&FR| z#8xG+cQoVFpq@D%;AqTo{5EQW(?JTM64==?xdQAbx-;>UWQ$ya$0HRi@dO>?`Dk4@ zm(*?@rC3{8`6c$9N`+VEAx?I6p*(bI5Bm7$wGkXvpHrSNLoh-`57unwc`9kc}4r&n#S53UA5YK>FM z?dgTH;fELT?*!x%rm|oNc{Zixi;{dG$;P6ow85mhl8z|avZ~Ez$<&uWBq4qE zBV>#qB^65}t06=#^8*7f#>B3!I|Nm4@)}mb<5J*Y*vNwk$?n2zgDWeY7qW^;zK}sS zGIglE8S>E)^k>7($bm&M*DN0L%eQ&Yz(&p*&b1>D&oVlSv6pwM;s-Iam8Z^a{3(Mv ztiI(R-6}&XG_t3V$Qb!566tc7AiO5lLKKHbL9&Cx=*~q^>tFdhEXPg-$lnr!KpW3w z;0(w*oWmo323+Tg6N66Q8E}aX`6XdFa%I_^{g^x}^L&wuP zO3?Ft#$llhyMeHYX)rct7Z|;V#?}BtG(!KSvg0r+PE{I!BphWPY2|O3jxYiTZ}bpn z2*5{rvTr!9n@>_2+5*YpaM~3zqxUd=dSWsXdv8`z%kE6o7#zeXM?35x#uGfWB zj9mVy9PR|?*EU$U2Gdy3k6TUIlcrI5wn5!(Ia3~pon)G#oC@pH6{d|0>?wEaeCk@c z2pKIP!y44mN+z-vcPWEI*+S07AU+)5lcsY`xrzIeW0uQ#{kyjhSupfyqT$+uV!XiE zP~eh(F6z0^mu*S)sgvNJoVE-yH?B?elxr*_@(Y_{MYPIyI*SM}7i^B{YSuo`xpk#& z)(L2@od(_3ef2JZC9fR9sJjP@_&#AXI+6Qy!W@XLOwpGQNyt45noM=XmDa6w6zP>i z%E&`74|MbV4^KlUM?p|2r0^%JUW_!Dmx|j2?*Q7kV4r zOWK~KG9fFw2=&PVbI%9?dKp?_;2Mr|W`)z+yEvss_7r0(!rlc#i4y>CcMxbD#Op8s zJeez#GDnz^h}*^O!sq}1AOJ~3K~%Fl8pb39ipH%m zqLcj6?GUpf+tbQ`KgG}jhqeu3D@r<|MKtrF=N6LupWrk#{BECeRGDow7nw;8evJq4^pW`U7IHv=dgQKr3LXONnx-SjL!o5IJka-O?Ba~3BUw{_At)U3~fp}E@*DRl^lGtFiVCowgpwmjfl2+M(fe3(U=nJ1bE{jVvWG~O=bBua61&E83i*cqL4lkYy7@bcbDF@N^70Pu*))1E#k;#jUX`r|%P%=t@p4tBSU4>48 z7^Y&V_N1TKPSglA9naX$!WO|`W_>md#=iJ zF5w{^!4uX77)HRdjxxl-%C3+!`KM0E1&{F!aCHnyRDC6FU>sR`-Xp!fQ&Rr=djRy{ zI)E7!h#d-Zkw)n8Y&1?>7<9RHDF1K_PVF_a6f?4BBxz?#NZA`bmACYhzhx_QB``h# zBe!{V1Tc*&L>B;J(vR#Bng`Qps0x=-P!@$Rw1C!YH#z-kWLpn$&x00nnpCkZ;`_&ti*Xx}>s>KhO8M=HOS$lV3CeuGlTh764rEKE!@qO)qqu8&L%$fpC(aGw#Mx^!yQ zFI~A?1$XRX8$V2o$2BBoeqoh`Wq6jkrSzX2fD5or!Ti&yXL!FdKua6X8VRsR2&=Ax znP!fB3_G3bV>D0d*2Biu0aUaQSe{EL#GrCEd{Yc9L~cIHX_AgK(+#9Y)`>6d^19v+ zIABY%M6bho$&7Xr?UsnfG?jc2UPZ28se}uIRZEBjs#T(zFw5}YJbTk`IpglORn=$? z5<*WSO$hB8v?-HH!|GhV*{yWFzt6%r2h45&2{urPdUNE{#?ghrHvB6uKn((?aKw{c z7}p50NXV9&?=^HD?b&+g%l2TbUpQTWPe1-iPliKfHj0ztST7Af3vJwez1J`Q>I`y_ zOMd)Chq6NkC9ggzlCs1hR;XqMNYsm-q-Btv`8+xozUR6`l68=bJQ&?sn^`2I(WM6k zLeA>S;dmS=FmWV~jOtPIf)`2-U$Voa7a5jmMw-xb5jATeHf-ufJsITV=kDdv)qd;8 z{GIa)vDA=#3X@barj&t^o(|YsgrfDdIv)?Ids$MI+e)NT)IYK^gsY znJeV?-gq#EJ5J2T^64Iq0e!fjMHdDkbd~=Mqvefd`Kw1dJ>mTzXif=2%l&uP0kR7K zjqV14MVdNZH7jMzF2!w`BX>kwo3a4r725Hhjo=e+ZMiPN7DpOl%FJmy_s?|>_PPoz zee)b4-e)wG_YX+(MKe2xyp9eUO|5dmE29iq9zorzOosV5MQOKJcb~tKrO}hFQ_1@f zu5EzhZ#R6*q8&29FU4q4N?pDBoJ*P7&~s3Uy6xEoMp@`JTba~ZMgWu{+DrXwAwD&z zuGm88nr3SuG3gTz^Z@|GrjIya%@m+SCAfxT+prS!LX*pF*#o$ZY?B09UZ!n))9=7- z0aB?;>p-(I<*_~B61m=&;4->0HEr|7fmF2RU4t=LSsIYWy13p4kDEj27vFxkmmpwA+*qtsRT4RU4gt) z+1@AH?J^DS9WSofgZrXYUU-@VH}_d5*zGK-S?g|XeR8e~kq3DxOI&w)aAnZQ>$^@7 zIDVVccT7F9&509;b49PYr%;v(x?AzldDE)@^0f4IXel1e+bvXv^ZbglxK2-$-4|*e za0FQ0*Ac89+Kc++pCboD?jBxJCZ5I1Nlrh!XF5C2fN)Q1kVuLq+u^yg?mec16v4>C_^t@<_1G*3WUdEoZ2+L@J*tie=XwN8l9CPN2Q5z2QSf zF@Dy|;kKVu^X^dnY?R2RE~a`s*tA1w8+Z;#fn&Y3zM+py)~COP z)93CGE~n-MNZADD{}@dZttK*$G?8uCDH?I_K{n$ExAMt`?9eEW*dcsf&GnPm?xTyN zT7@-64bt1GFYwoc0TZJk&4!Z@;*B1XY+BhRGc@6Oz$B*kr#aXIl*GV}%tzW_MMiMO zTI|)8wo(sp2$gZZlb;_R;TMQFUHSg0uP@^WoIle*JXGY{llkf}LvU&&L|SU677Rs6 zs=g?%Is-?&-DgVi4u_!LUcGYkIHwAyF?N1SsVSS^4zONr1op(@IDL@5J-Y425lHLp zD0Cz6%=AM+{Li1?mw12<(#u+U(j1Qao_~JNk{mdIUq@k`8Ep6|L+X>#?j3xcn6E!i ztqwL-6DmQ^rMfvCgAG`mg>w{bTnC_o+yyGqq-!8=dd7>h0^e#H|L*@rA=Cj4J{Egc+?zxm&>@Y z>Fq5G&u;N)8y)hy@Hg{*9G+7=owiOx2MIKP6Q(`HZZGBS==juu`IdBTTqkOXU05fU zhcv`HsgtHHJ*)>LqpdrLLY@3T6WH6xmI{>^&-v{+5bipME52wl_s_7;w;TR7_X1=T zMi!=FvFFkBYgUqlY4SD{TcTi5l9>ewl#4#EeR=0jIQW8SSr=!^W}5Nb%?|^g3#`;d zY=hKuXDf3_f$Tq>uV*8!jsK1)2cO;EF@JS(x3S-SUb%14jNlk;43A!z-blmLQ0$eD zy>1+X4bvg~{tlog@TvpKg{|r8w)j^bS+sPBZA0f?*b~A|*tEAe-i};U7Y%~1=j=hU zyUi}o>Y0#XyYbWmksX00SKA!Gf;2T9e$!J=QwPRdpVo!_Vx3SMzo@r+byIZc(Vq{I zqhkzz7}lY^ZOW;eQ@J$yG<3@5aNjt0`L=PY!4xH=+@)DtNd|?ie6YHd%e>lj@tdcX zn(o?$;hl~yda}-qAU_o_rztQ#5*EO_a&mh)tEQIdo(?Fp9x`X`i8>T0M`gAzYVbzG)U)v{v4M>&NQYWz0w{H z45U!!a_AumB^EspHLuB=DK`W6iMFZu08AQ&T)GfNH#S7X$1p~h2UC(uxmPJ0q;Qpa zasE*n38&bPA*z`8(&)w4bjy;=B~FWdd4k!y6w zuJP^e0PQF3)BH2o6Po}md>j(rXO1jh`|d(+d}$r)FlLWlV{bOvtqikp2xh1Er@Y3~ zSdL00!NBv{v<(_TlB_>Dc~}1PnZ`{cjsOt05vS*!M&AbC-aZR`z^2hd+lKE#>ISQ1 zq)RMoA6t%Q9{vr}V=2l6OK8OXl3o zn8q)@XYdWq#$?|oV)k(Si-HyM$XN)nVAwIU4}=lkdx$qJS;8iOA9_G1$%{10Ve49S zL!L3PSA3@0A@4Sv+=pZ^fj<;#y2T-c&cY4N=JYE4!Wg;am>A}yi_dT>4ai2)gaVJC z>MzHYFC;`ErUdLlZ zag0Ab5uXWbnPO4Q0lscY#A$HL`+vhew+v@S@Wm;Kf}5SBGqJHG7KZ5T0$6DX zMZ)SY`1?FKVYz%sJHll2;DZ??%z`DVa)Pre&`Vc;LwjGJSxI8cII5UP@b?$z%DfE{n9+ z0aDKh=nD3b;OnzKE5cnx0!aIaoxwO!I{+)9mu=U})^fk@+?@kou-UsAFln zhLUYBP<+lD*T^O8Xu|u3bqcwUZZ=JrHr|L46!ZM@k}9!L>I2HgxILmTR|L56DZSPLv=31|ppiFz9IySL+S6fIL{K}FiX)Hi8P0;vkKqh!0Z4m7F^JkWD zx$mVwDmn;*lU(b;e}hox06->=r`qatN^g~{5-Mo#KHA&nFwYEig_TNiDsOm*LRdwL z8xgoC%jesLxb+;?p&(E&xNp1*T{M~St|=C3AwoL3JaJogWg1zu+)%C4Ql9QWj9w_K zLiKkZvB6WQ7-W!fAi-H)R~X{T5m^Mje&Xa5$BG+mEYfb z@U9ccf*rV_PnmQM2QOlN3`LtUb&9GK6PF9+7w6hg#xLATOT&YHY$+t#Ah$ejcjZ3| zo{Vg2r*%fwe|z`!Ja-{aUQ-vtc5l~IOK8}#t92czFio?N zP=evwKoItM2wd|IxVywfRB-AbLAv~#DZoNEUpRxXi3h1dCQt^%p;aErSt00JeGX2RYWHMO(n^*x;2aA&XK_Jd^ zC0~%$zt6@klQ1sGE9sW;3Lo1!$!^x*fvBBKnASuB#E-D~sl;a0(1f_a9XKMG&K;%b z1#k5_XD+cbESte3V^a$A-6xA&C}UxN$Ur&`K82?Oov-@Vo}b|sLt0Uoh2J9V-6ZIo zWdviC)Cw3iuRtYMK`QD|{5)Rh`s`bF8o5-O<#mZwu0F4^gv#Xt-pIF!SNSS;<=<~3 zhL<=Xgd33zQ)gbT5o8@9`fw$BW`n@{g{I0|T$(HV+hFP(RtoYXmvEzFp7TUq=)ean z2+H}0KV`B(i5mx@ZcJB}bqI~%NEx`*kFJ7l+kdNR69rto8^06GzoKpP;=>LDK}H@I~V8Gq@@1Nxu3<&Irf-^X*Z37L@h$WKI{$pl>i)}D4EJMHqoe|7_*ojN&pf%7gNt0+v`>Z% z8S^lb?Htogl7dNoL>Q1oj*fuvm0?L7G>OTx`Z$p)hcvHp$%JPs!)Py450znMkKQB5 zrd)V}o)Othoo?|S9E+5&&xZj_dSpNdXazr_7_R9#DiG^gdEL)O+&Wv&jktlGK38x(Ja)u)NyrD3X05Ga41PG>}zBIGX3K6CB= z$}~W6Ea+tA3;{17=Ac&a$shP;jlkxK%fH_HygLY1foOp~7Rkr>92JRgenvb5`WL60Bgco-1&GD&e zdlzPLO#8lY%|Gdo8ts;*viXZQFTU}`or{t}a48hFJVqfYvnkz@i*(Qm1=rEYc}&@s zLUja`y_bpnE!DtyAt7^GhLy@|KRU@r%yjXJ8%*Lgik8v5J_82ZfdsVmz_Uq36=cGR z6FBhf^?T4{wMTtz8O1tua62yk@(pp|lD38Mj-vK4Fg#?cV4}`}-hRWDCku}=6smKO zE#-a z<&}0RUG=s4m#SgQZ@hW3Ie9f&I^`o>u`YUKC!)Nl*YSGlv)`Zq9;dKn&J;>!%9l2t zXw?=rUYr61$jh=Nx>SsAob1*6{yfuqcd^YBv}BW#cwi)BD?7e-{BfLv@D1M`Hq7BHqcU zD{-zGna5iIdkHYN3>dnN!@}kH0Kh(4_BOP{Gg)(z+Ywhh!%AIjtGw2f5(zmNCDJk{rc~l(1E2%O5N}~*o#HRGV3^u)u?2)^ z+R%$kri7pO{<**2)$7f>)X>r(OL|)-A4{NVDdWt4&EYI!;KPe@I8qX~y0A%Aw%d-P zwv+c5hPrUYk(;)oMxp@OB&1v<^?v5%IXIR_fW-*aq-~v*Q1c__%13Xpj>+7>OKZZy z?p-ut3L;#YCT)?pU*VHHvK<&xXFA?E01>WX=VmcZvi#u*VRA;iaQC1?rlXVIJhgP_dH*a6lEFjJqczs zb3we{4Zbm_@i`i=+$G|5pS?!}5=}pJHD|#&UP3yiw~ZkVLD);$FpjYrA7R4nBADmP z?fs9ia7cqxZSNIG;utZVzvX9RwAas~)By*`WqK*O6{^tzjG)A&86YJYh~wF;!UP=H!5&C) z8g}m6qr0rxkla^&mGJ;dBwWThn6)&Gm38etaD!?gk~hSHgDu7!FEC|LhJyORk_^cQ zX4dpBOxsY`LDex#;}r)+=ABFFYx@8IAOJ~3K~(Ly`oYJ9NsL?v&6D~C zB(&waoR?lQLdr!+w!u$+k@=s{k5M@&G_jTP8}xQ`yHdj%o| z6q0-)4~0v_5C9_uDm?LIBS1Xr=Al}mp7r)fWht7}qV0%5KE8(fmZAPD_VkrYkJs*MkG4i^{JGUqpASa`2;Pn>40;}Mu3 z9Jg>fDv+lZ<{%eXF9M1%NHNG^amc90Pj8DOn9)J}x9C|1uq>2dc&vU`AC|Mj?vu|7 zk}@G@_y+C{%#`8U*uLBN!I>c3(oCU{DMgdcgzY?%a$JNmC~a|CoTgV7%@3Ftv-mU5 z&updK!*5~g2w)-~U{C5rUpxOaBQo(EMHqa|*s`QNPPvFTjl7{(83QgxBDv1A(S)dQ za<|{xv>+%Lp{1tXNOT!YUT&L4c4p{XC~%WiB8w4uD-*i_7Q!~V3pFEu=}JHWhVM4Q zR^*x9e1)wV$w_?i_E~AV=S#}1?X`{?G+x|e=8iA;1h>vZ{W_8`D}o$_yH-7P6)p@R zA)hV5q@9J3Sg1)c5+^bjOJG=~8+X+-tNd!y>_wkP6T?Ok$X<|TEsYI99sb)emz*%- z8f>}=ODr`x)WLtpA(rTn^foS_fn8duPaaq@EuFc>F-&|H$tvfRfxL3@St%eoqU3o; z3n!yc>j}QI=MT-`m0C)4lwo)r`I4|4j9+wUVL430d}~u_h)=$$Nm0%ir#OL#O!nr7 z(@RP(Y21jl_?N8WaV+Q%>KQOmWSzfdv9v5?7<1XV6X- z7gk1B#fb;*0-yjY`Brd5NLc01-FjfqaDowP2*+b+SY^_3xaP2b%Nm>(??5Qo<=Oknc+x)&(=@|k7+)ISdR*HBQeo41mzROHPlA)kp0HDEo_mo*VPTSv3jk)U!ioSaq%a@k<~ zs`8{qST4C=pra=e_a59ntAm!1d;aq_r%A^!kklGbSPYFN*0fxMWQ@G=B+E}h%OnwK zXsCesc;FE^o>2x3`K@22WudP^21Tq?MkwG6M2`m*8HO2-hp|(dHC-J8kdt1`HR2kC!Isb2f#*3FW=gW{KeTKhsQAL+Q)5~ z;T}SVEW^5~Hh4SL|^eZ@5=8BMk6W*gA7wBri_VXMDj1hLPgo4H82k4zI6Vu$)CNw8pYOMb{W zzxNZ|ev$Sc3?oAFiAN3}6od!=xumepCZF*}*C}S^6&36~MjClSkIUQx7vjBS#aKWy z41qgtO}kzvzYIki#0y?|)G#y(L(@(+*EHjS0w~{IwDEfck;J9dm`{e@hxF8)scW1I z0jPva^3xC=!uhtrVU|-yx-4c$)w!i9Caa+1c4?hy~LZ~weaV&kbN2)AWpdEeP82mT5*KW z;`%7bJmtO0mK~DmOvPHBwu@uhhPc6*Da1PFaEWe2EM_&ubIQ2XgtW|k*5S`0px`I$ z()w;1>{rO4ncKvJmx^iuw-`wpyrpg3y2$5GZru(Nt2~=7&(JV^vl68;$k-P!%xrmC zWE{9Xwk+IZ=a?fa!-OgsIA7OoWiYCs*WVTX<)6Fs2G@FQS({|m`8N}hkBrP6u6 z8=7)4W@+)S@Zu3iz6>2Z^Q@bWud+e&q8U*$<#=qzD@8IxTafD+ugC zC(|66NjhT%zIm79wxFjh7@Cx>M{wKVE6mbv+99H36U>hcaHvjAHa>w!oD*{9yMo_a zpw()M58_#-lxJw4+8x=#qVZ+75*D@+3DIy!bv*jL!z>cQBiy6U0iQHQzNLo{EY&!| zVUX!=3=sT$zFUXaor{pd@YPjD4&qqRw*t?K$j#0-BY@Rc6dlC?Gv!$P-b*ZbxwK+x zP3CE!C0-?!PWX`_iagTmzw|wQ;mwU}3~no9iJ0F?cTJ!p`^q4MsB0L5-115?*{$SN ztT1K;hp*b~wmOF)pk1vxoZa+z5pcqBaE-SdORHs)-x*peF#}IVXn1WTDMxe?x9l3T zak14^WP)kRVF3=riNq@8UR@p8fn7@G6As&kg=YwVM}uA=_pS1<%s0KfmoN>au=%kUJslj`AofAo`qFp^=F7}5*I7#H)04GGPbUTpRzha zYdrweLE^x3Q@KN1RO$PdDIV!P2y_r55=LQpT!v2w33{utZM@w zNGlmIS%0wbS-d%jMM|mWEx~CtmSMv+zG;>#a+v8~SlG}itaTJ`3JFvk4Ddo8^ zXcRiq&jXi|T-LoGX<=N_11g%0uzZP_FhpFqY9l+>1yXhdNn1Jv>jx33blcJmc4+Lo$Mn#3PpeA4NmU9VTGZ(-2qIgxt(WRs~+hD48n>7H9 z=-fwL0ZKTE2Dm{q8Ua~bQIRjt3sYGug6H??^^YIjD9sb{8mpjZY4_Q?sT~*}iWn_L zj%Wy=peRgGdRdkg=n&3r#dnv+y%>vn2fU;Q$h^FVJ<4%@ZqRTYB^D3Dd3I@}(xPl(#yk{olrD_n00HAdDtW7nV@MA`u(qH;4VhbV2-Kb?96FRt z4~tYVER6sSf#uw^LEf@kHx7CT_nwjI$%SzO$C{jiNF99XE3>~&cuyefiI=Xjr`cf=G9S``sSAfPdlua z6@=pen|Ub*d0~IS9&sXH>V&dZpkgcc)IqR>KLE;yuQR!PZU#4dSE2&1ah1{%T8^eR zF8x);)_2H3=(WGG$FItqLF(5K$VChJh8d3sf$wwon%XPT#O8=>Q zVMmq-G{zBl<3onqL`yc$E44|*vz`t(BcW+NVV@5Gvy10qpT}h*>wt=qMtl&3SXicZ z%uZ<1)SLB})C&`y%epzIfQ!Evr%hgs+W2v(3|Uqu!Dh&qW4S?rLpIrBc#Av;3&TpB88)2m=Ew4bV=@W%Cba6oPcQg9{)a<(J7DVOT+kxVKgj; zZJt*N;YC4Khi0m~0Rk_Y%;OH}`G7%E#kK5l3>hsnUMB*b3&T?6I^wntj@02tHW|_T zkz%B9Nea=)#3GdL(7;*<34+NZhl6lf9W)m2fcJ5QH1R_FJ93bB;sv-5d2 zTqlNUBqlv^A8BNrUjt{}mA_PdrqccQK6%muhnrh#mT$M%!q2wAzuLfZIqzaaG5ANS ze2)z2@yzIQ>rjN?h_I0h{Fzva@;~56Dky1#V=*Fkz+2bKo4LL4WXeXkt3CqG#rl+( z{{+kwK!S#5Htw}X?H6`KGDJ-;e5V3Zk*Oq{yS{q)TtvZ+5T%NQrZoLR0}~m3elvWfY*wLHUc!Mbp{}zAm(F6dgAY|L2dT{2l74BnbL_SMpSXuL+xJNJ^7|MDz z_LXyKiBKaOiid7WEk6-84b0w3TA6!lxN(5Rlg?pO#9Kya9Q2fb(gpsWAZ z$E`5wp|yEIl4ulF0uxOfyrtB({<(k2ovO!bqzoR*!Jxn_(!b9mMeo zq-K2ob&cobzw{4MAt`p`@u`tRwoIX7ER=7QtjdfZWgxO;%TIOZrKrLP6j+err~^s; z@K2n6%g=(j2GRaf9QF#M!_{g;la_#^hw!x0hJUiw5R@-!+@zsM6q(<~WZ~idZI@odt=5ooo2u7L8EO`l)b_$pH0Uq|1C9oIH zK^-v{-iB*>!#D?3O=3nvW`J|ttiO4VUbdqNakE-~!c0=>4^9YOGfd~XvJ%g;08Klb z=p-y^0CFj8;mR|-adbHysbO7^@-k2>e;IhkATB@(Q)O!?a?u&^(hkac{pR(}D?ZY( zmFvM983aC%s315^^App)!YYkJIg-2v@G~6=9m%x;M8P3100`3cZWtcdbbqsMM=yd1 z>Hug55%M=2^bcaBse_Aw1+c(@kNnGX@X`@gS)qF17+F~(I<%RK=9gv;$N@NL7JD1@ zmYvsIkPRbsC3h67BiTi@=r%mS*ZV=Qlt)qA4wn-ANiVxx-RiTaNY78oS@)%MOT;UB zvX@WqUX^WWTi)37MGP62|(` zjuL!kt}(-?gYJCt*t`e|ixgC$rwq2%6l@A}&a z&@R?!%A=2;(q+l|DkRZ9LQQ<>K_St2&+im9}4=6?D&8^kbR9mUX^S zR*S!K?_HVAwcwVHNA9UeIh_Mqhb5gw=Giet)8CK&co_BYmtKs!`C@k9~q3eyzhwcGwgjx*QDqrELIsm z+2ZijOitNE!5Wq3MPt9%FMpKsIrNQtY3?2m{QAh_A>#2{3a^ftQ_0pfca~$E#08D% z8=H4N_{hQEJGMAk!)MwF%?(c$w?=5K5abd#o@HZe(^=`Cwm1Jv{Nb^NGY{@Mefyw; z$poMBBk!i%5pP$wJL1$Lq*ruxet5V;YDd(Z;^vK;ALvY?GaVZ;sZ%UYC~`w)fZ0qz z0TGLzfGoM+^IW_eR z?}W7>x!NK^Lg)8d;A$IAZIAzkm+oBtZCg}~)cm>GO4=o&KFm|i!~LWVP8PJ+^Cw8> zoOtRifuzt zHLLgK^F`xdF3#t9B_wxPne})kpSnrl+(q~51m=V|XQ$`}wmw@Ku&&1SeQNfc2dc?L4y9;nR=QgVh@E@xtfMrC5$e3Q}V!cVk_M=4L=n%(}3Cz9`kFjMX&6Cu!4_u zyze2;mD_8_1b(F#!*BKd6s5A?v-^BQ$zA_Xg>hpt&aB~V5O7hs_Br12q2D(Fp*e4s z?II|Sm1{MaWqw+ad7}Hc3(Iiap(ph;t7OEg3)R<6x&C@Wtcu_^g)PC46i+AeuBlqL z_ZFApi+87v=0vYdpZR3>sv}ml_2%zNgmRW_T0Q3V%tKZwYtp6IYhr5Zs`E#AwuF^B ziDy4}s^`F-%6?Qx#DQx@L4xM`5LeO7O#T zBEhwD#j4NxOV;3zv+}5V@0Q;BrD<}|I!A+sII(Y^5(`x(78@x>rdkt@B%Tb(h-BKv zyjd`pJE3#8ZjjLdd(`OuC6f#+o(ra2UhH@O6X)9d@V$kLQSd&CzppDXec3IXRPG`6c6Lch0<3iW14pfM$Jr+00UNSLah z3a`nO>df60#z*7qYq*T21Q@h@K7_BU5Gby5mEL{ooN`pH(Lv64Q{wMDo1VKmI7~N1 z(~6_%^{K#cGx_^<jUOd^Y{doICjq{reGh=SwVV`(%tIK|y z4>-}Y*;Bng#b+^IDd0T2qk-=67u)>{A3qMC?(P`nx9jc}*K7C>>DlfJxVDOPHx;X| zvJJkb#oeK+yjvz(#8i5jUlVuqy!;PRiotoik_wd)du^A#x=}PkFVUz*Rb$-I_mB5x z&CHeiBv-W8tFA<}CbL13Ra8$&so+X_-)&~)xX{fuTmK^AwS%ZB)3%P>3T=K(&yH}V z(i4vgw{Xd{6U4n!fyIS z7HU_6W?qUBXK!8mnQncg%e|GGFWO(9!}7s+n?P&EBb{c4*@DWN=`X}<8s==@@3$=}Yd$_$D=V>EDv#aGObj=>!}eq6p{h0CYb3mrjY zUrfJ=XTEF_wa(9YrbzSVk_+_bSI1-(Ih*lx?MwN#_e|rWlD)^%TNN19om2fQs$c6$ zHy=NDYO+6Fa@QWyj-LITpVqsiwT>0achz$W4KK}=Z>x;g5^ z=V!l6x)wf--O~E#Ci?{pVNdyvmVeqgQ7qEKNQd$G(-*w^7UX^45}bNb=WfBW)Y$f; zw{xZ$hi^RKys>NlA-AKV1zF5_1slg7_N&<)>eOvHsm@~B?6V#$+c@icuGoICkK|A< zi+q{g5Vc!#LY3&)=&6UdejxNJmA7eM31p4Cd+%7w%Y95<3j`KM2ivW(lsL;6x$r?1 zT|u+sJr|Da62eU(aoH(!-3Gk3dn`A7Rhc$*;q%Ih51Ki%$EBVL@w*)w)O;xVSZw}m zU$gL{Hp3Za_*Yf9#SB3zHmvBcZv?YW9*gGi7$3f2lBNaM)#VS=Hf>emb`MxFdUN%9 z6~!&VK}A9>N2�XoSo#P`xlgD0md>{4?xI5k}79uItyFu}}$6j8@n+`NiRqCE+W) z7k$)Uo3@B~-OZNG+Y)TM=4*E?(75-4YaLt3$EM02cMTU2FBkob?L|$;#+|j5qTgQDIC&F}?n+Ro zWUu|S^X+_v3TLsv+H_;h=ks{)r5nZw@!xpD&sVQx#Y1>CM~2RW{^=DVOOb~)_?FSH zvaDx^+3Z;Q`kBquGHs`JBi7hTU63a>QB76aqf-B15W1aD<7Y~e}_4>J5`r( z$lJ!qmq+aDJh_^SQY@31Z8-NP^`O7wy6+zlQN4InXka(*=DT!HMF|ON@mC}j!tN#J z&^IQXPq({bGnwvwT%qXOgL5^?8f!P(^z_KDSZ8#iIh0AeaTwLyzooF_A4xT zb06OQw$-qRwI$5p@}>^=*cZy9M{i>pdsZTYv86XI>DVqS>&qIAI=dt2V~@Yze>H6V zY3s_4K}96JN`4 zPnZ)zU+n97=a5aq`^E%kzWnVMwO_|3%%2z4^unvUeEQh^d+oN%ytdhUI z`5|6MPDyGo`!{Dy_tEm)e6+Sa_C;x>ZTF0*#S?4BE4sEDotxS{>6xPHi*gaM(nqC> zJ$+7Y;#kyWX#PAy*mLdG&OAn`^QYYJo2=cT+G&tipY>2nYzuQ@z`K*(9orJ(%NatA zN4ePghc_t1YFh^D%g}8<{E#JdQ7f0Nv$Djwtj6YtvCf*}VdY))Y&@=pvo6nIIm?sq zC46O9%c#4J`eoBW7&-wh)pQPB?TgXrkYaw}Nq29OiOMn)q;)(it95**#UEN>ELKQ!~tn#!#Zq8D-uM_t^ z*?Ek0>^8i|c=QTs%SCtZb>}J^X$i0<-M=`oyk$&@8WZ(PVlS#9>rijW-6HTyuGs!FB?K%9u*`w_;I)j3=_-08h zj$^uae74TMe3?73sgX12(_+W?1azES^#s>T#Bm&I7*qs%3qB{yf4 zxxPLB;gVTWYSOMvS|N4Uw)oD{OR|)m&(Nk5#kbVa+?K8BidsU>20M}X;O&tKW0f8$ zFXi6arQ8{`E8^_~iyiseiE3lQQd*CF3E$3e#XETQ9>!L@aO3CK8#;Af*$W)1z5o0u zU24#CvBuucLY1roV))6)G9MnBrf>1ef3|AD_QR|u9oNtLFy@jKR|->Gcqbz0VdmS9Ps)JftjQtthScFh6hGZ0Eq<{CRjG+)4I?fRbav z`PKI%^0HIKm|NQJpAW4*JWY}tMd{ovNOCFx$0|NI>^2>@C0t=;ke36CS#?( zPRU-p)77QDSR(FGg)8f%7j8=hMIMYg&3GW{v-PX`oWfhz&x#!8x_)zZriyZ+8QKz< zUX)~3$?XC8En_i*t>%={m!2|H*8}k?WjV#ZHo?ao5Vt@XsX1cYidf;A(R? zZth-@tlFl2*2n4CmQNXRZ@HW}%_SPjHodheJk6ohI=$=_+p+WA=!Lq1GdZ$fjFReU zpT;1n=e+a!*@ppl&wuimTj1Kmb=;Co!YXC0q@J@~j$4XXy_LGb4tgc^k1BSmsT_%o zlXuAliaRQd@k!4rJ8u8!`u!`JZ6^;sqqEM)pIj-|CbL7Ru*t__jOQr{TtjYkbn7!s znm;vCTHrzJr}rE=hMq?yX0K7qTZadb6b^1N$aCe;%wqQ9V-}1Ue!!dlS(a{ z@0VN5T5ilRJ#S-7&k}}EfwxXo+!2u~8$Px&Z*Q2Fa?-mjT~-=((i!jfZ)53snrF)Sy6y$b()@azOUw1wSS!~W7D{-82uRJW^NGN- z$*f*iaiX!rJX$g4&IQ*;v(DcXGM-)$_);X|aS1nT{iF*^7$$LT_gnh$#mDD!_VYhr z{@S#5)$RT4Dl39KV{;d>om`bReeCD^y7dZ1t0H|)N!XOx=}2>B&8^tatF4}sY$P!C z$(T#2zMj`KtE+3mI}+WwuQpAXG})5nP3+!l>w4PmZrd3&NqqcU5ABedtMz8vw;lW< zEdE}qb?R06{MiNd_t^G_=pPQhumALctACTdyGfNMW7*3ywo8-!BO`5m7>+#InEm;{ z#W1<;Y>XSG?WulIb^oOQ#*VzE(2IKv?yQ_M*EmV=_WN@T@0u)>udh5b=BT2DttNnt{S`lZ?XF`JKBDXZ1;e3Dk%zZyi(H_UK zX{}$L>ZlqYP6@Au9h~=Fz2ibUY|V0tSVnRlPz^qNu5Nr z$ywibCy%7HR;2NJ+ul`7kS^J^JaI+V)`007%N!2l**Pufv_G>-)`rUImxWCKno`3O z+nnU;Wl(Zu>Sv`BjQ2}6FFc~fnY-M-Vs}z-quAzI=8M(3-|-37<=mSUdIdeP>G)## z69%Cr`L!30@70@9tH?3$cD%3J@*`)tSFAdr6{&B>Q^wY;A!20pp+tDboT|*D=G!B$ z$f=&W8~eDkeU-73Z+dWSR`$7^<&yg*D389cG)6qr=H6I~-D8ZLXRVe!(kkkak?`o_ z%HaB!#=&PYyIW4*JI5LoH7n6rh5IzigwIl?*F8(soMJmJ*(Qzi%j2qJjl$Qxs1p=b zeY@q+o{NE(3}B!TzmC zv1|>;`?;&c!(-lxig!NMpS@wtR*|0F*X$E(k7O3O-`@1v=C+!uQj2cp)~sb$Zaz@= z3Fcv(`q@6Pav3kqtNuXRB-5&jImP0OmxKvak1t+X;XYrR?sFS=c+#n-Y@=-URn%AC zyZa@0$>qcJ7bc!a+%vXFR&)vKBEq$(%uxTQ5M;H}M%{naW6c%F+X?R<)s-?U)deyQy7QtI*+ zF0ba+(UlelAM8~#R@~J6Db;bmo07=ks_Vh(`&pweT6`-EiTkEoS6`Lx z-&V0VO0X#0yn|`{Cvf1J>6*wqv`|aw4Y-CR`6z`?4|FPv;50;o4>AJYI6KybEf`i#x9Vd~MmY=EGxDZ0t8~WHf1Ar&p4fge*y?86k(#aNu0<^@drC+Pnf%!}D&tLJ>R8X&ovi0RmOhfm zap0_vTW8j?k5>dY;fkTKOWu84T$tba8drMpv+r>YRU16c{L>kqIB^DJ8_>75%GEvlZ~n|h5Q;Yy(I)y`Ufrt9ow?wjUL31m9IWmSHO`lQ004WU-| z##7x($vK>W@C0ud{mmviJIy-T2Y(S+Y>@{_w{&AtEMYtUROTd$JnlcQ4+-J6+7ZD*wH&CVtgb{RZwvnH<~D z!!mp}4TrPi0`K)SZBVXr+p5f{x9v>l@#lBco03%hKX8k5P7a>5*Hkp@w9?i0FT#|2 z(ks2sKRx;Eu;kj#(XZpmSIx}}lU-eT*q>E3dG# zw~v2ZDJoWZS#Nzv*~W0j&zny~PF6YIBfE0r3Xv_Mr|QPM)K}Z_(7)jg-(-$ajd4Lc zUI<#Bna0cLn7uh>27L{SZi>nq+d~PwU&F?nAM?V%cy4Fs*jx6x?qjZ$y1l=txWnqH zaP(_KrY>kF}2|XiTv@VRSxMxJiC< zRdu)ZS`Aa-ak&*H(!AlX%`;xFG=Ib$reON2yMx9P= z9`i7|bKA}-kGkvCEFAR@%DPTp6Xz1l|3&LE*ET!$3$4o^$h%zhuC%qjyo}diiIIxi z9M#SeRtJuV=B!RT$nf=c}~J zEoXHPh+DkSMdDEDMGmmO<(PDkZcn%PcznLYK1@`Cyeb3dG#Z;PufTK+NU zK;X3xm-FWr&fUJNRW-@-+2y;VB-XY`scwj1RE=pUtarO)=94t{!Gd>jp}Xf*`D}Vs zmctagcBAq!1GOC=pQhZ-Tvp+elG1RnUAf$`vi(-z!#Iu0<6j@|Av8*{o60ROQ2p?E z?D&NK9Tc;_iy?2LZlg_m|+QJE}z;q^3!;#+we zYtk>g)<6EFS;FQ*4Ubtg%b~ICDlA*;gS`~&oL3T_-*aOu+b1sE-qm3I!ac&?rnw{6 z??97k=Avw=9_B+Q^cWJa=3CpXyO7TtD52FDJTbNU6n;jhcfs}sPY?Um-D2_rqj+D= zuNCJ0y3*s)%;L+0p86-PK{a>lnps0KAAjjAGTy;laY!^PVAeQuoMQf3rFXTvKHgAa zd62^6)xK#~bB5j0CXpB=pO~(asfAbHHDxbv%kJ5EG{?6|%QZZ+6UqMz)w&x4U}2K5479S}Stom6N@Y<<&+erjTl5Zdtcu><+UTd0IO? zT~i&{3DTZfy=9#p`x0eXJ=Wg|xfCfxhHX3fY*~-wxEaz@GJ5E4Y9ERE9Pzcx^+ClgS543IPc;_;gp3!l zSUJ2_*dXC$Ei`Ibxm`}(wWSx!-RLD!@B8+=Jf!w!lY?m1L&u2|llBG{t&PQ@7u}rN zUt^Zu;+!fVe-*>`kov)1g2xSM|ejP};nS(S&4CF5*NEgP+mGw%DKV`l%V;iyVu z?zz^5Cw#mkY9{yKS-LNL`>Yz+b-iX&vH)M~>4f*07Tb4DWXjro`I?#bJe#`EhqGDb z-f29Xvt;4clDP|9rFd!-?<^D%E4d@IcGCElGo1oGzA&Y1efXiNpiTADDCXIQ)$ZGo z`ZUIJtq}aMq`6pBpjF)PoMpp}y^UT+aXDhT+;~OLjYgJ{wQ^kB?8nb4T|;;AUa~=v zs*LU)PKDV~_tSSt?K6s5$IP~7@#8OUMQO|FE@YpLn?|3VZFc02*f^ZXG1dAlV@1Ve z#4O8u-35-^(pziqdGsqCPOE;qC4=rws}k0UnicIyckqf|s+U~IQulX%F)gt0>bA10 za_yZ75$xCG-BT6H+Z|&zZZDbmG#sC?U&qR)ZN-z4#){Uiuf4aHr}RXux;)x)TBlE? z%Ym+%z;EBq2EP6}-=v#&`);P(7G?=u97_7yMp~H~R-5tYni(A$Xc61Z!_mbH#|n3g zEOngx99`@eS+94&-BiZm0B&f>_cd7%%s6x`vi2ajcxgIWdwSyd(LM1>M8ed@!_m!4 zLfz5B)X`bIzkLx>!x#cf{M-oSu9mD=q-|;B>fvndgqwsK(sy=qA~>VA3AXy^%frsv zhTw_vaX2<~=n`BA9@eOg)bj$A)koF$Wg=uGB>KudCB7qB{JT3#O%WUyYR$;oc`?Dw z$)mBM)2@*Lx-sfnm+ zAJSR`Pa;blU0qN%4mTDZMA~(ntnEE+RNJ5#mNuHfXZ~N zojeIRCb)9cfD+uJ>%G0Bkf`A!gX1Qi{mJ*FTs&$OyW%2W>EhPnTyX^40`#qkbH@4L z>~QWlPhze&&Iq>#<*&gxpj=N>V;s4b3A%GbaFFYbVC`^TG)w8ob$=BH268*ga5iMD zhUQ>=_+5P&BObV*-tdU`Bo6lHj2=Q4D>=syamI^S&l|S{XNz-0Ex4eT@VIGY2tCBV zHHeRnIBg=VJCeW}oIeh0-3q6JTaRkFA};sC_RE2CDg6+Peoopf?qtSj!IK1e&Rd4hMAYiZdbWE*`ln5;c^{FbQGj{yu)p6JdR55qtGQ`iXI;zkf<) z%w*iglh2q0`_C9h2g+}t9rm~Xt2AJ^V49_eT@9UcwpBz%UJU>=w{{sQE>ayV(6 zB9g5J(nC08kuOm~_wq<)a%hB*Lw8aLLKA%}p>HKrJ{Nr}pn7oh+xM^1iQzoZ2lh*A z_)<(7)am6Q)2o9x*Y7t5_GN_wS>n-XPU7mX(t~*kJ*0d7MDN}2B?jhU9*sKglgSwM z;|)}-MK%exCNaSKTOo0lTOCl$w1G$k ztiPY1Q-2c;7!MYpaasd;Hi&uwcWU`qe2kUT*rUS}9S~K4zR)V4PLp?- zHrpZJ4AB%G$4j#g)+))%`XGJgN6`~$M#YctbVIrZ8cmA&_|YuPjTmde41{FId^G#O zJjwS_3}}xt`V5X1@dyKwHc;0REUu72F@h`_PZdxEA&bT&SrkLSw+uQfpb<+Kr-irih)tz3jA z{+PvhA!#`yi^St*{wCb9Il*AY8;EXoS;5aw=PNJBjl zPy1(c5TRl-YCH~$KSl-(u>yRkTPtS&)Y{`Gu1NIvFGjOPEJpa=qYUP?4$(t-621Fu znr&j9K$qxEp z4$ZzuD`NNobA*BMs01;Z=pXC*<85)8En`#wt{TKSl?*X5ltq}sYzsW15;}km9drg? z3I0$M^$Gq`8$rvXGakj1B)RrmG3F%(a=ky6g)z+=X_Y79IyP7AAJqnyg6AWxY2eXv zCoS#4qQ8N?S|JR-Uyp$Hy-*&+MtDR6tycT_G?zI0)Ihv|2pVRl8pP27=BF~~1LZJQ zXc0LEIT}dbz&9y$#v@LW`s)A2^nIi&8 zL1aA;4v)jQMj9{pao^YAr&tH(rSPF{$r#sQ=B|L~2b+*bnkI|RB+XGoErV^yAucH* zUTPCZT`6=m7jX>c`I_hoR)uNxg~b&E{hkDB6=H_|)fAAr#1L&*fczP0T@dVXc$zRh zC(Wg#P;1f%ODTjaP(hkYL9QI42jU8_tT~7ZN;52Kb16BRSYSFslGpdrq9k8=a*O@e ziOpPoZ0!mgV+XFm^>er1eyNp%1q{z$Y(`C6yYYy#v|7XT+8EipFNzeu=M-hVR-)M| zwmJgiAXu6mMf;dXZij>;tb6uze|X=hhg$STaW$+>_D6;Ns~%7atE}Hc3|w2qdiov$ zR#d55DUPBnI;`5=D62e46cHK0_YyR1R4xcvOhU zWswJgF-RKOnkLbMz{m?_uvVyrJO}s>utKPziTY8Y*%vn7>DOS)6G$M>iOpne(JTi1 zh9gBOtJ3UcsK@AEdyyL8F=P>C*oWjXz>g>qDJJF5K^z$BMC^7`-slkY$K@SZ_3=R6SaKz9cZzGz}n9Oa6F&)f4VVs9G6^p~(|$yn)zL z8Xd4A1oItOA5uVf5VdQdd^w_TmPK|Uhdc=Cn;fvtnq@S`yjk2HyvjbrOb1M80zyQMT+A5L=1Xx7Fgr;Q{8GEzbZ%(y{D zFynwZtS>-rz9-YON&K z!esQHE*)xG4YXL$0NQEExEHbyDfBY0C-mj{~#N?|u2)|_C6ALiGfi84q7VeN^uTCaqt zl}6VvGFy@R`v0OKgS8xdvGs`g1ysuubpqC(;OPj`{0Ddh^CVbvf;ASHGr;PDCV~cy zAg$B^zt+&~YpDGKjy5#wQKAfXmIi+QzuGUXK|y>3pYi1LK)OiRm{!XY#~`ptuw&9L z6Fj{F^Hqo&;E5K{c(7lHFTir=qBE=)!YGDEu^FwtsM{~#YEQF1HT#9yFdrKD`A?VH zV9f&>9LW^WmKJIK9{h_A`oQ`EX~z%N6=2l?*7xMl87xT?$p`!scqZ5*1pkCbb2nPj z8fsI3<2su4C{gA_vlga@pk<(K5Yxg~Pg;8=t(|BfZG%w+bQIR}A@+qa9$-LYK{xSe zZaPp8{{PMgz@}VA%s0}02aLI}3j$si9X`oPYF7V>)fcnIi5vARZ2G3!W?-o?pyl2+%SD+TX?2bMd$hXr)gQ9ZG1Hu?jaET}hGe zphTK@Er6_FfLJC&zLzE6%MtI<3(e3OgOw+jDGl_K{8JwP9%zOY&l@nU&c zFA&{fd8D48F4jJ*O90-GhxH-d*9WYUlj=geL85OUmJn-Tp;)YHq4VTIOtgk zNz)o>oG0QVPyo-{exJJu<$vE(KPJQX)KA4ods>AX+}m6t2LLmSJ1~Y}TqH>vJsJ;BI{(L5YYuzN~^n@iMj&_0Nd zvARE^6C_LSx9d%0+n!6b6tMMy8YxueehHSR_8}~1^Xme-I4POGytBl2HwJ03#%vn*Q9p5 za2_a@ClF&efFN=4d+1GlrNG(2Z9()0HA!UmMtV?CY7tW6JH2%KBj`@d%@URMrnWkcZec21~jJSS*LcWy)Akk^t;?Qm2tr z)^8`!5A+1>llTT)!t{SQi%=M$MZk*npRx#8ZN;?uM=b)@Y_YokeHNiWw0BI0{St|Ndmu0?$zVtdIWhj#}V1_W%0m-{qqf`({GJSp=*@!YU!y-|#l^C*u~t0*ju8 z-;1KOTKnG}xd3kAf+-xZf%TX6w?V(W9XEMsKx%by%A09$Ni`*F+o?;bCJH!z<6 zYZ;^+2$DAc{5*GLQ9W=Si(rNyG5(F~|BdT^HLeG31HFcJXnAXF<#%|k{IhWk;QO=D z{eOELLwc_9UvK@h-g*_$3P^kU@H8s+gcb0W6cv8|+#2L#kq%sw*2uA4b&_S0c97vo zI&5a#zncMd`eTog&dtByO@TJBmj6i{`bVsPh7Y7&Q{ef}aN%b>!}oF^t$+S={exe` z)~5d%4nVKi9P^*S0Dc#O&9r}93x6h-|6T`3yFx#!1HaxsX?^FP>mPLRSA5a0a{%-X zi)MaC2cgdI@W}m<(vR!m@4m=5eX%@!SaBY#f$_FUMw>bI0Qz7 z3xW%jFdf0pkOwcZfdp@inS~C1bQlruR`cYdYe=$VA(~GUb8xs|@)?jJCU`s11@iTfLhv0IsTG1S267AFzM-z55|*GF zOg;d=ms|j%xJ14NE@PSiB(UQ`ai35-1adjX3%DKFA1J_iGR!&h8I$`C^8Jt#)&L*? z$%J$wzLp0r;VnkQWI#S+`a6U-P|i&T5+$GePiM%NgWF$%fh;9ofUQ9)Af3>^%90bX z>$}$?&5`OOe=8Yg2vWfRVMSdspmn&!7>2b(RG?|7z-AC2W*~il;M{r9F&!NWgMcw2 z{K@&mS2k&oZ?=)oLge#0^0`kXp=Eb+CU5~#2A*`r1dW3+0|UY`-N`^kB-o+f#^fxF zh`ug?h-2hJtTl`^WkcH`SP&Dwx7i?HZW>9-P!(A`CfxjGYaOM)#uB)qN_jO%FX2}qS%cBB2YK8FDEx--ZP}2fg zK^j~F|58p$m;^E9aUkapIb~V}x!MPYsmHC$sdh~_QO7t2fS^l?d=cSnJ7$YF?I4M=Mxn&nS3*l@Jw>v5GCzFN*X|=RScNunP(s%K6GGid$48U z&@b6Z3Db29Nvf7Li(z`kcJ(;D%=azn!h7e1{xrhqU5LcghcFDt!swWZ4t{h%PzeYN zphFBDv(X`ljydQMM~4JDV5pHq#}AWH;}p<#ukNEktha*ZsMCMcI}8W;fsvdKZaLm5}gJH1NVayAIdmA??BE&+$#Oe z&2cIp-}DMl%A-ecUJe8obV9wt7ap>HKS9tmBtV)L;`{GwowKFSGR_DgSk#1^JI{ye z49){AJqb>Bq)3+$-5E$5x!MxY3kY$zCDtDH1h0XYC=*A#cU`lVVBioF5CkY=L8dY0vks3u{< z@EIS|3CF{U#y%2)ZYg%#)33-eIDE_8e<+m?%cGj@LaZ-@ZmC?A;Oo90|=#HO>nDOi?=VjRjYtqWLX==D~R|wi^-FFLkpe zz9t!4nT0M$K6Id`fy`IGn?eYJ1ZMBNxk}{Y!eZw6;~Dwg%%P*Bd1eAGqsBeSQ>Qrjb^$UV=* zPl1y20Rt}>JXx$0G4tT$Z6{@|X-O5z z0sp~Hi&EZScRpY|gFQVUJe-7b8Q_t|^mXplWYMsO><^3l*A6OW%48G&` zNPU;#h3PY%E%&A)uHlb_X1#H{qNgc}860u8_$GbIOa^EkocGT=A6WKbk)j)gmu^(w zH+hNw)GbaPC~H`${--$~HzW?Kk&oNYL_Vb@=VK3gJkj6!*u0(3R=;m|0yu+3K49SK z?`r4Gt}ZtS>rySqS8xoZl=n|KA6WP+yHT`s+rteykygQv?&^#^XRxOqDM7p6#=KkUWU8~fU_Nf2N$z|gXmH+tqVs{3*i!)$_v?uIYQ(&rc)Vwd z5OFJLBtne*%*WJn@UC7#$NZ^Ym-cr8&4cs)dFKOb(oN?1)%z`u8gA8QpObQ7qASW8 zR!z+S03~ca_2<`oT9Ie)A~lBo^p%vrScz#b`HY1fFhhhibQN|AsenQW57m+%UUxlk zAkxHiU9wS9Hq#1s_gUYqyV8-nnSstzT&D0Cw7$|wXF5`QwA+0?r93tSXYB3e=Iy0L za3a{FIXAI^UicN#SSoAMIICC%=?dF+|G zJH~sYiU~fiA1GFARHYQ7P|9OK2rTk&bt4hji|%m5CmqQfWN;79>B5;(8rU_Z@_~7M z3-gqsIsl|4R>1#knmyn4+)Y{$>ZWEH6-x<}^1vDV!lodg0?#N>OFu+2)(AD)ApiHN z>mhP?zo4#ah2ZJ3!wR7G2k#<4@3hnv@So&X&18}tV-RvDd}VN#?{Z3c|GVm% z25dy>PpRuqdDvIENA{Lo`=)k|FIu&n%Pf_Wx>DMZqLeq#!w#dezum)v$`0Px14h$Q zS)ifrqKv|$lZy>Qm^us0Y$hsD%KP6{*`2^flzu^F7Y!E_sG+uh*u!e1xo|cqR_i6= zLg#idl%JubvXmZgpp=Js*kJ;K-yXw4Krr}$5R^qzT>*dTrSZOHDxVkZn`vlX`eE}W zN_qdg>Y4#;MCljQ)eVi&RAX4RpNU~J%v~26j6bb;l24>1SZR(sC3U5=>qaSWpog7H zB()d5|9A`wn-PO|G>IKU=WszK0Szq*Ay);Migi-1&U_G|O-Dy5?|)Zi`#L4wCbRm9PQKG>dhP=JEJ`X%X&aSN9_C?(QPQp`KPsOkwyDnxjPl`4O&3rEYP&;84 zC3S^(bnw<9rM!V2mPF*=@vyM4HF%2_Wzkewpkcnrf&;B5_N#~2l-99cN!O*6_y0|0 z2g(pu?O|074b8-#GoTak@p~^1ZvuM#B`J+XCnIYgN3^?#7I=toVD8%FVxXT04~5^ zbh7sJBzQ{bT00YTJkZyASC4fP>NZ}k9^wwTVlreUKn`rlli1R1KC~Ig=jLQ>L*Vmr z<@0p2Uhl$ZgFQ>^$!G0i%V+D#=Zc^__&f<--frT|YA&u`1U@6w6Q8#y!Isa_g-_Fi zfHp+?{s@B4&fA5EFSDAPHA0Dxlnh;Mh>RuQ@-0DKJCXBHvjah~MYe&U9^{2L82r(; z3_372fx&4I>>XVeIa&Mt1am;1+CJ!SE%?@>U!edy{P@;Dd!BrDt{#X*j))UPh9W?c zAmxZxq@Orz5$vqJok)xLAmo3`{yblR&%^QhN9-J6skH@aMy+i0FlAuM)apSvL#+&! z=BSmy9vHQ+ts9-StGswUs`hkz8n&{uS(3ujI^T^N`bu-ELSXVd*m{ifBjt)(9 zXrV(J9YBN{IyBG$jr=gXK8DYfDu=a8 zh|T}9EH9=T>f^?CXq@U5kBThlYN1M{)o-J6%HOBbE85ELLd7Ai)R_~-yV!&tQ>D`4 zw^7OS_o+NR>C||~6-@eDx837kJm>X&s#LE2ZB)Xa*ZgIToj*g*eD_WV?Q{Oh&nxZH zo2XLB@gr0Y8$=oYeJU$5dTZ}>1!$gXxpiG(0eb{hD&u||l?H#G%H*n=)SgQX`aAcn z%V2x1T||}2oxhDrSn2-D8ar{z{3kEp9#%_}wXZUNf9(!cDkFazl?(npmAAHg7-pJY z)eK3r`Fv7z0}oXyqbX3CLe9e`8N@%uLU~x|O5D(a|4>vyhcY_mqXTAyZ0LZ5RfIA~ zeHCE|6)IAjp$u+N4Z&hF7MMIi!W!$FCcuxd2L`BTiC`1}Z=o<8Vi!oFq*LEFp7_Ud zaC?rN=TAPvE(*m2R71i5Va`TO>g2O9`8>2QjH81~aJX=Cogq}-M#y7SVrMcejw;2q zsM2OK6ebi5O{@pvI{7mMh&`P@6saU~6Buhou*O({m{6f!T~#&z_aocz+mS6n$d>#W WWd96j{}8f&hO-papd>^z<^KSmJl?PX literal 154302 zcmcG$2|U%$_dk9`R7grugi4E@$iA=HWzE*LUF+p?UHeX16Ot`Ui;#q5%TBhUkg{jr z_g!}X*NEQg-RJ%L{vYpm^LUJTz0R4L=bSTV&YYRot#WU=_|@(0ZCz{_4C?@FC*eSU zr%YZAj)^>2#P48zDqJYRL=4deueQA+RPd+Db8C3{RC<%+FhuLJ9iqJ~kP|vb`UD%QI-g(z=734e>t(BKf6g5Dud3z zU`$YX=Ohef!Tp`V%@2ctToi!?b9B0Mr~dRY(U+LcPBm86LC_qu1AXDZj=`{DD{36D z6VMwyy{b4ny%GY6(6d1>vNN%A>{Nik#?#pUR|TlqP7>1bw+0V`5MqJmu1>zQV34M^ zfhgyocV=Nzw6duMvk1}_VTCre#Mm&4D8eO>j;5BzihA~zFwH@$N$USuQBg`!RL=r| z)`NYB#&9w3t0#vtL?CUL0)1f-n z|64P46H^T0N5d)JMAplHZwRnNP==0*CXP0y1~zhf5UQyj5~d`P9ZB>1>PjdK3eCDR zGz1cfK*LsL4p(0M$GQLoz0(Y~yhy_WY=c?Z0Bvf8+0`eE?x~Um=|1ug{Ys+stW02I zoi1V|``Dr=G(uHD-wa`Zfvrd}3heJu+|t+-vNs>0J5s=a_kdI_(Fi@HxFL)IC-M+3 z#1J-saIH0AVCj9-1#3 z*!Ia0k)wcsM(a6($wHtpjxecL1ExDvTMSAOWn+V|u@RPnh4K|h?R5JW6`EB?NP#uv zHwM8_Mf42JMG=;m%u(CzkAQUXc^eps@5wN;aN+Qs;zA!W#C4{l54Fs!dWe6Dn zWPBD@(2}*p!02ke3Q+9T`p#7`H8B5PAFa28^K2XKd}ObVIs$138^P}o1@h~Q>RE~) zz5^P<-qc^{fYvxfMOj!tor~D&>mw10rk0inLzp$wMcJLf{=`%>wK0Y2z;4j+@d4FA zO)YIOXgyO)6%?d9XqL&6+aD4Dj+BkKp(#cW)IRC6>I&>pUlw7kXW%G_u!I&I5DUzq zlno@X2?`C>#hBV5V3=52J@UUQ%OenmP~Wy_V}!Cf0;z;BvVjGsY4ZXBf7m39gqw|! zY(M2^3>a;U6(dlM}MnHrn!tY}flZXxwS?GArh5F!NK zLhD=#(x0BC0Rkp8`1~U;9ZEA>+u(dSUF{@+CAzC}q{^VYdGKdoP8i43Od@MwMYW_e13|n6rfiwcE8R39|>G!#0g5{J4X23(!rca87Uzw4H0O_L?1I3-P^HWXu8A@MtZhL%wIaT&gD%44oV*~ zSzA4%sT1O_MU?8x(XpdDM-1YJ?uO+V%YgnxAfV3vN6RrHK)VAeXa0bo}D zq9QD;kdUOXFE7r&#RD8$8^~l}+HP!D4?(ZH`hRNzBv+ zlHPGgk51vkB0!Jg!0-OGIm}ZNCO@U7{gw3bm>H?MSR8T?~ARy-@Woc!L zfqhmYd<8t%-#9!x-}e_5n4i6ODp0ey;rHzn2bHY@6terSY+!=0(1Xidg}o_{A_OZM z_)>(S7|OsFcxow2BNVI2`g*vXWo&@4if&m#&i;~bati< zkf2=w@(93-CO_><-5Ik3!oU`TP}N7EjX}Wjv)#g~`WWx-bm#zWUJ^*KM2c=moCga> z0*TVY{Ls^Sa`z^vxX-E;<_|4_iI+e_?&~KX`;1`tD**o+=VuoyRz#iwe8o;RS%eX0 zH@J{Hpb>cX@ARJ=TUXy^1eUVX7}{G*_FC(GMPz~a;aA4b&9x{4$AOpo;nmUe`TjtU zdxRbB+Q7tUukh~(x$FJ^(r}{Y#z}C7kOb~f=HhTGX#LA}>vQO)3fQ1`I@{T5pL*5V z?X&{>uBhUg321d4+Fo`YsVNk@ost;54iI*UdzE(<3oPdIK{~M6{K8a1 z*gzW#j2H1~8Z5+J?%voW5f~T;`u+nj3|Z(z@bfygfe}z1XaxHH5ywxxnr8(DC5Nzp zY;#99%SzUGf01^iy-Ssq3r7FFmZBbH+!&bKm@6N!oA1QGX2dNwUvLKlfzcDw!|1^V zJ+i~VSfL|~EfN8pzjh)kMLmoO%<6og9++1-lpO+ik{=raEbehW34jFnKQ!z$e@Gg1 z4fMM+)=w{SQuhduv%Cqb((*g7kDviVOH3ia76zKw!1A|ZkYL_-PMnL_RfJ%#+4WTj znJFw_2H#(QUvw`*Hc2)qdsR)JTAY`_Y^g9U_%efua3UMIME=fDU?xWm{c9D7)jl5oi?@ z@L@X(NdgTC1ygy-_Ymk8=!*r`LUlJ5O!dy&&r3aE@oxEEMOK*JfwA?yimX3Z?BUgF z+N%idI5c=Fdlj$#T#?@8{P12yHW(4NKHFYJwx27yWi2i3sP5O(GMn>#r@c1tpU=Z# zOB{!R&)Bhponx$4po$#O#Q!=H^QRje1?y(tVODKIR}uj4?#Cj~G0GC`xiC*x{o3!0 z?~4K0%1%`4r+AWKKLa{Ue`gdHbjri^>jxzr;!JR&9?1 z%T@;VVz6@>Jgy<6vLnL}(qH{)o~voD0MqKa+R8*AED1Tdo1% z)c>lrcdX>z?jPFgb|+L*gj~A~?97FyY``3$vmN?3YLv^;g#kH!?86|gNR4po03dtB zzqZJQK(U>jM-954g6uSj0>=$ki;J^rp8@RK0@ zj<-kKEBDL3r>o!bzy<))UY)z&UtwL4K4W*E_u6IczJv7;virQ(u5kDLE5wWWAG}V8 z7q`n>fSz|BVIa(<-F*S`bY=G$e0}-g#8AG{}13K{RRi|2XJ2e1}Eqb;5_>cPR<{|dHx%m zu0MdI`x~5toBzGNfs+PE`~42%AHZSw4)YU^>VJTHi34U?0w2Yy)sucjaN&`uFzstKM>V`vyH{ z>^|%K2RQS;%+K3>ID7jx!S=tGX9mLA+K0umivwbj*4=0J{{RQf{r)7r`@#*C)VpHf z!}cP?{WbshGc@eK#UZo5(3p3dgZRs2H^`*l`&AA^1JM7ZMZo(V&h$PUAR9Q%g9PHM zy>mkt&cB!ER~*J2l%H`p4*vHzdi!vUcAwk-0FJ?La0GGxdwCH1aQ0%7#Qy+iYM&i| z8xGA~9B_u%JF{`&{Uy);%iciq-`U%*ID38v;M9OdP#XIKID38+;H2%Kz+kTq{d;*f z_UR4WpjkorEtCOH`tCZ_IsD(^Q10SX?BclY9xU+*{|rv$FF1b0e+H-O7o3Bq{tOPV z1u#E*dh)(Uf3QXX&g4FH5v=DGhChYIQw z;Oxocvx{ST;oswcADDtMg9PNUf$|=X>n_d=&A-Q4`2`2uKY~3Q%umK{_sC0s28U-C zCu_HRa@s$G!~2Uo_4I!R2i#Etz3s{4%JgS&_aLZ&=T-F%)0}3koxy}`<*=C%m{FXp$t;8^q;{2^Rgc&NaoMrfKROZacpG&3=a7E zydOtH?$6+W56$~=1m*t>4mghN$GNERXK=uAWIqnR;-A4e_zTY5AHu=?1*i4KpOFV_ zqx{`Q2v{_5U8{{yv<&`w-ll|2@uw-{55Z0i1~6;OJ`n_wq#j28ZyM ze+jr-<$o5xiw{6$U|)L~j2t`zpAAajQ$Y!Q9w>oN0wwSnpak|0O0ecY3DyKC!QKx_ zus4Ge?42Ni{=xnR_8ve7-)KNd3}w*AaVUd6LI2<#;DbH^F6eV_9N;?@s0U&vaCZ#) z0eyh>fCpl?Q&0wYU>yxX!h=3QeQ;k!1!ZvV=Y%rggE0X8{J%=j-$f`>Lm9|%3CdTX zd=ARAPzLMhJe29748Cz)fHEVLnV<~r(rKU!z7YVN%TNaWQ9~K@31kEPfcBtI&=+V6 z=zs?L0({Uf=pWDl4fF~4dl-NYXkcuB5AeX)0Mt`yFui0`4ooT>?1g|6j()1=TwMfZ+l4bnyf3+AsVFG+ah3{VrH)O9hXJ?758FV#jrH-f>aVNznkDh^mh#fR-E@PK1u-moX)Oq=&nPKyR6mfJ_z>Sl!}h9sX|b#0 z`ut-mw`N&am18rci<*t7(kb(xm!_{xv=Rs`A|D^+cuKTw{0e0?5lDF1Z9OY4GTmRF%2jGISCz?Ev&}#~G|l-PaagUfn@Wc5Y(Pl2z^xJ20WPyi zxl8wAtAlL#V7Dcll1g&i^{E3Ig>IG%I3*|-U7?U%r|xcud<3U_(vi>lweog^xJSws zb*ZdY>uc|E^3A4jQo-ENgPamu)3byF zB!LefMReuX4-+?3s;nLju^^{U>LstEGt*$UlV)ajbvq+z=xw0!kaUx+W%dl7tpiy# ze)uQ;*~m1}OZVZFlnEpn8j_JcEiJ0ulbVziv|f(d2V zto()zqX;j5V7DhxV92GHx{qAjf+QcpB_EaG&NfI+ZC@j2m}e#lj%j$t%dvJp*c5 zH;(JPmD!p#Kzoo~G++{5G(K=UVc?)mjVe;MvT@X#>M6G`MH_jB8GTK4&sFl8SLd}& zK1#lqBv=uSE+~m1BD^zS&1q88_B6HsL2rU#>N--*rYBzjPDwYlLZ$= zDZB2`(iJ={qjDSW@24p3Z(k+jwW{PrvkzPMpT#wcNW>ECRIs7GzapH&z2W`5M^r72 z^Ay{GcgoetBa7&lHhH}lSOZizRH~dYpYG9EB#_40O^^u09ab!CQ@((DbJ3psi{~h= z*|O~zRdztcW2d0cdW9vb8_!9TOh;3#SJcaEb8I~UMbbZ2w= zO|M|Elvnt*twml+vYa^Lg3zk*cc1w-ycu}N>|Gg7UA>|-{ps!d8!mx&W0#VOs|apt zum-^036x4DwyPyc0y@`VhwIKOdBrU|yuy6caau~1=Hy`2fa8Al3pA=*GA2O9&6Nd~ zPUN$%B1HQ z6m>JyW}NL8(Hsj(W01UoPq)Mk_R-NZZaru=y(wh1gSizOH9osra|jwsHuJUcr4m!0e(BNs(l zZAe|#yGbOlG&wxZ@Pr5{n^Ki29uTlFNzBWg6Q^>2;riv;;l2br*1*iEvqF&)2$}A- zZzk#j9O>+fnaoP<1y*|kCmIy{>ZwPl5uvsOn*5*GWjnQ+mrU15yD1sg58;U%Vt z73S-;J)1#T#ZI5}2WQ?y;jzJ5AKg!OHiDZl(|u)Jidig%s7 zhNR-LfD@m6-+l6cGx&w~Kw|V{UKpDa*&FN3{d5+nRH#A!%hU)0uCsceAj|kbCie7c^=%uLCdreH@ z?fkl%k}ps1&J8}C8*1EZW<>87yV5`8bYE2~px`Ji@S9u~uj(4tnQ(Se@5mw2wKg-a zdmvdn7H*-#z4k5#Uc4NbxgJgnIVJ!6lfWX7v78& z2H`|Kkn6GQopOWhWZ?gwASA9^whAeGRQpg4RlAXQ7SB>zt*-cU|B( z%kA)>WlZOu{_>GBimCHcf|QXs;*>hDnc`!qqv;v1%Y@8RCM7ph-E1>#mojWO3og48 z6{5*U#tj_R*IF1+pI@||IlZD~R|oG@U+q^(w;gLOu+!`rAT_AVCY-5dCSq_{NUOtmvnP)QoudO3QOG?VRaZ%|xV@ zTBw%VB3`Jk4}DJ7S&QhIsufN}nwyZLzM1ABSNn6i*Rt~zhE9&$BCM~LOyUafiE7N1 zw@@+umKUs*{3b^$Zdt4Klr6%pGw<-lyT^$(oL(!CdphFk;(B@K5?Wv41$Q}g8saDmNJhIDzWg*tAw&->tGRG&g5?F>XZ54s^j3Tq?mat=!% z=&>y#n^LtL4W$!`$EG6H@|RN_!{xi|5M@^^T8H3Eq0BQ0%flB$^Cj^|1_mUzZe@<5 zGzYE}+m7Yj$3sk;bO?9zSF?~kC5K7zdNo1~VMs~mU*r*V_z-Ctxk^4kAT-NZ)V%}FvR9ujH zbW2tHwW$W_%)n0tJVT#*ikdismZuYx^UZbg>5E-4p{AwR7JI_=^U^n_^K`Vw%?n)@ z>ESP(vum}@4FgrX`D5pwPY~ITjk?@cb~L1O58w2=mEYp~)g!`GN#nX6b9KsF7RayTDo1%OBJFc@oF+c@OwpIRj&}%^WPB{m&2w?g zv7LgC%|1Dq;5r`hk$y_EwZVBMeDmxKn*dpiLZO?5lf|Z#&Svecb@i`ZJyU6UoxN)5 z*RoRy*rgGgqM~+n6`igf;V8x?x%_eMAv^cHytt+mndD94vv_nP_S;|PTuUvni|W(J zrWVdoYl>5N*lAr5c>>42q-~<^XqdsHX{2ItaTCd-qL8# z6%0!)W^@n6l^FY?RkAXaw|&Dkyl^=qzauwfYJzf$znw8}HRcwTK)+~@9epXQNo0xj zi$VmwdPf9vhOOgNf=yx$oNv5gurDDC@%U4zZ;s;-+?F1@IHU0OBeJmt7#!=3@@;FR zEh;ucIJRx*gqA(F_|`4jt=wSk^+)abzTb$`9Ab#l7T0cUE!*ey!Pmc;R55nvs%lQu z=60`S%Vcb%=S@beU(dLzmGOGEwud8qN-&&$TJ_OZu1!_p+A?n}BT>tAo@^#lB!Luy2^L8?4U{A)=k>+Y3@+^5=5eV%i3q!1?h@(DK0TD4am z9N##wCtL7-N}Th?Jgi`zbEdlS-TCe452_;x0%S#I+a29brCI8C(-vm3Ikzl(D&EYS z7h={DRUZj#(4XDnKeH%EtKPNdF+KdLd-J2D@r2I$yIc~3x{8EtVj`y@dmYC`+E%yc zd6J8PncO+nRd1G4m1A4N&0Q1o6p9gd^ZeMS9-pyHb#}(r4f=*BgkLmq$D(YWLxj+x z!xAO1Rv4^p@wK9xD?E*FQ}9ze*|$N??&iESE$446xnGSpf*q(n1+Jgo9%tKhW@nK` z5pG$2?l~A&!cjccfT91qMkljP$&npgd(xopBvD88N}T-A%=@kc)%Z2d^!Y;gBTbaf z;N!gRtJ$3>3vSz^F4u;`%7{;8Yz~~$ojIJ_my30E^h+a=6_5K(w`67VJDpgY@_9U8 z!Yzs&ALR(%XD;5-W|fvqf1MV(`J6DLKs{gZNrEVrdtHpf$ZYh7<+&FD-7Oa6aV65C z-!3>hFm{dvBS{N+zGy9*o!CnH)RS27<=fymr!68C-W`5xE1h1Mdbv{qB@eOP8>bPy4FYSj`PM# ze<;|~(uXwY8+Ks5n+_pq*cr;itA#q;_#d$H$At>?@3zUQ#s)9uS|Mc2ORr%%$S3GJ z1(qT@r)s|@G^{vN%tpwwgZZOjlZIr&jV5Baua&w|&Njd=eMA^T2~rQ)sdDpqtYe zHn%Nlt6O>;3U%6BmJBAg(t|8nO_e*6T^l)weU8lFPWBtaE?s}auxJTt7S7X<{>$m7QEL|n+O#qp=ifP&ql+daJ1mJDAzQUsS?Y&Oi% zyH*vLsUCIeY&I3-$t$kSX0l9tJ~LO_^R+gIsU?Ejlqx5;>U>UmeGbtkrG>#c8z;G$ z0W?u+lX~~`=ixhX9cINt0nG(PwIaznPU~MD)J}DL+g5s2e`Wj}HBLzz%iTo<-L->Q z4y2<+xOFehOXszcveQ;W2@Nv}l95{K{+d@xH+4!|{TGvb-_8x{WWe6Gzbi#Oa=+(T z>*YPwpVPB47HMx5UPwDL6rj3!_QJz4c*OVOzGerIGo` zJ?t%!wb|R0L(iy{SHm8vLD5+l-kNK0sgF14zE+>w@P$IH0XN=<$pU`H89X_k-Nv=XADM-w_Lz2ea3>qhQ%pNMC$}(e zNtNabRJ0X!;qq=~*{+X@ zt9I^E-7psvb=d($(q~?YjD?a5<9v6=sYD{HTk>Be6*87Y-h(eq^KT0n4RZOaFR%Im-#l+chGemO+>{pGNN+6g;V7)VY3Qvw!L#M?vCA-rEZ@!nJ~eM;N62 zzt}#`#vUaiZ!TpnN$wYjk>uepU!_!XOjC$fn(k!GQA(59{(OcX>GsTh&E|Y}LOCJ- zb&8CrsXPJVhL-B?DdneIxI?jrwEA<4t?ky;=!zRwmI{~nH^b}#Z`U&pO~S`!*zBKn z=L&UoBcJ8gpdDRfVu?zz7+F3wR*c(?q?_vMd9xk2EcvNB3*8}?y4li(7aZ#Del>rj zE4SoEhy^&)aD^8(xwlGY6nvT6c=iM?Fcr~YQuUZ=_#FH|(H54Fu((lR{Du)MUE{h|uV$e`YrN$kEj&|HlUAQ|Pvv=YPj<(Z znHLIF-7|wPdd~1;*32HQX@;~r&6>~9t!w3brPQC|PJVZTPXO;#P+4I~ZO=XJAW2+* z)&qS>$e?%SN0%lg5n5j8DN*|F@Vv3v(HYW$2epacO5Cf8mp^%{t)0wR?9aRR_5G$j ztNcgmTT?Cds_Bct0(-zX{+5*3isCvm&Sr;cY0enuS=yji??2azmU()Y<9EBIIOh>m z_Y2e3p6{`UN0)Q2&r}7j$DcS-@U7N3>f2xj`y%;}q`;SIeyZH9lg{4)GfgerZ)C0VqszYt^#M3r(47(V9`{*w!f)EZ^Ez_xZ5F^pMT@dUqHT6S@g%Awr|4EfBn?+I^sk`RpZDto`}@A6w>uUp(5o2Hdj-fhtDTT_Z)j7 z-pWnVb}rdxyO90%xeoOla-y5B?DQ^C=Z#pE$jjj4R?%XQ-BCG%ZQ6c@!6245r^Yfa zoxPgFri#W?!y3W=dFWbuj z%BS4yNba6nxH7!ux#gH?>rb+A4@3CyWPYfCi{vx*O0v8BCiG=ET77s-w?}6m5~PGH zIo%Fir9P|7LU%H6Cgp(AiZJF5k{ZvKX-jSN2(IhrV_#B_AXMGka9dX#WNGR=1Tl@C z*;|M3D|DKD=uh*soNF@SySpCpLB(A~JJ9x&#i(`jJJDdi#~aTcpaL(2zn;j7yS%O8 zfd6FN_BsQG%#6cl<^DqseWnv(2kBHIj%~16H+3%rvb`{+8O11e_$!!Q@(oXzddo$k z_W4Mt%>2M14(&+?ybI5;YoZ#@*CEC4tK*M1VD7Cpid{eLdRdi_=;%^tVp*Dxi z@#e`&tuA?2lh-TSxnl1?`!a9+OF%6UhGPFY_m&%B3))Yr@le`4&SKm9Z{ot6i) z)y%lgxv^Hu;}GV)$QvKWn|7k&vEo_s&e3=JBQvMr4tPEURtJRZ?GBk!pFPL@_ONOp z8R$#bc$BJCV99Cm)$J0^s#iwNYn7CjF?kvayP%QWDsVaz#&`OgoYM;=-n5GtOD1rfz>t-CHZ@51B#1vEpvD z4OFKxCz%f5*Kxyh^*oP18#`&&pQbb19qaY=08ejp_=oo|yhcl?gXpTE?Zu1*!$751 z085!_yIfPES?05u7e$mQ!1KJ1GyfB6fZr;@_llO?xPh z?|2`-Yd^&F8cT25f&)fC=I7mZO7^N8uJ2XvQd8KI`c$_VpN%-L8x=i$!aR<~=FX3> zdiW;$C$2NL^pTehEHdW`gkKzXBf3}4>N;&al)|VX)O@zL&jm>^G>qEt{vx7XNKq^< zXreWn6m+{Rb-*&{h0Q?^8jbl$ulVC*WT_l4AJXR2B`QDS%fX%Chx6eiw_Oy>@_2_Y z-7ys&f5YpAKC5}Mpy6m-*btMNVH#69qJc1CEB^2&Ue4Pw0@vRuq!$tk33Oa(lNgk? zca@ZX8j*}XR^Ruu>(I?3z3(_i6He*ywW)=@L19g1+h(!78535=H$92>!iZAf!*Iwb zYSP+YkglZ;>+V;!Ru;eaRZ`pth2{ybZ@Q1K3ZEr2D>AjFGFcG}?7E6KP+4Ls6)$6O zVP(xEB(bJf7Wb)AzTcN{KkZ3sFBmz|%85k_#?Y052WfscXXqP*vos7+nt9^W2Hu{h zzlU{qr7uV^TL4G@tCRa70iqyu$`zeC2TKpy4bE*+qLbn$-bY;@kQbw&r}MfLz_U!e zlK4p5Pl7Uq#F&cE-}7VWL*FQB$@tWFx%|T&4Ds;~k~5hPx(%3oku_1a@)tj!M zoRolFaY70YH_ZHKG6kEGP>uKD$EKBx6skp;2Oc%uJcq$*T<$m4ZM4W|df9Z5jrLlV zav?6=0Ky)lhCQUdg!gP2hS9>ie6Yt~H6JVHP09v4e*2ZT1AP&9Obn!4$K}`J4=EzP zIw6Dvh-oI@5nvIVbfU0-dFyrGZD8=?VX>8qeCJPjsT$gM-KhV#9Z2ifTJCI}9s9PV$yD%@GM4&~?eoa>SxbLZ;aYF0Z3tODx zXGr_v>I*J+Dg-MWv3zx!1B3G|mYi%|Sg_+lvfJgHun=}cxvX>N>r*f~O5KjOIWv}a zUwJR@t3332coTP{UK*k)Wv}^ti8)6ea`~-Q;^@H%KQa1-)K3A+gv*(V{!-{lSpi1s z9_t3T#@^=BPc`tn_?k)3@2<1t`#CR^IAGnjU!l8$4Nt@S=wfi-nOCdPp5w;SwYlbX{Vqwx>05ThZ(oGlw!O+f5)l33HnYlz zrmd0z{d)lkIRA%t{h}=i<8yaTUFPOUHh#2uF}#Y-l$YiP$rLI6(FGb}{YyF{#3DE~ zxBDtCN#fGXROtteIvy)~kV)Vt_xzDfj`$1%i45zjvbNhIal+gJlWeXVm{YE0_X@J4 z${hw}8mXnb=Z-zZ=Evh%^M9|cKVio=+fDZLv5`q?tIXlVRm5TQ%m z=W!o|0 z0)zTWX@)GW$^{zV(XV&C!tZ(O$FdRS592uoteajnb%`06_7|m3pm->nEiivU-MYN{ zmL8T5Dpo_~G;dDU4MEukqsnT9bJxp-dkW=DSu5OxcDAL(Uz=i7x!~ddAGj_0stuZ>AN=doFg^dHO^ZC<(uwJlHLvn^Li-MvlLq zKkl?S|8zMs_CQTa6#U>uU^K5cWzw4R^&D>);fcv2%ZbezV~2(l2aX&*HAQn$&iob zoevRg4j!CMid|o>9z9fvs-|hU!k|4Jn@L91&Po=@E>N6lYQ{HvuqSHrS)1d^5t1k?B*#VC+7C<*wD033A0SmLPlOMO5~2*3`5c$N<{iI9rHfIC)uBszu`uN8 z(aU@KE(@c=Lxxi_33LWDnY^LpR%P8qD@Y#>PsKwq25u>H)H0tV=MO#MCGME#+Isfz zwzJ-V-mvG)n4)My`H9087mH6+S_cS3#Jb>~U_dS&XPa|MX~UBZBhl7J4}PV2dqR^j zJ8kUFDI)cK9&HzlV|h20-~?_;GR zQi+s18lcuzdD^jA{~`04e5ahEq8sE?VNV!zt6vPne3T`r*T)TGmFs4>;PQF(CfQO9 z^2}- zTZ{GeknHHD4LkcL%#b@)M%dX5pHh}w_P+IpK;_rB9>`nyvfn$#>1|hZJ$jA7r75YF z@0sUvG)||{yO;OHc`WrS;uFbnmYXFSc>V5Iy^TG5=z)w_e;^)P80-O!G$9Yk#{D6J zv<~k+u0@YS!XNLDU@C+0v4pzAq%Yj13p^+K_OZ7#!To0Djf#6hugWV~ntj4b2MCx6 z1@i^2V!s``Kvd}!Fm*TOAXnx4UdfNka@F`c1bvU-Ukg%R3AUYeL1fMQl&9T)b>{|o z+7lLgE^JD50fRaZT^@;HvSfp&#scp=u+{HnA0rdoKu>A+9TdWZMi(ccx`J#hYqK4d60dAMrxoVm-v*5NO{Pi_xxgd4_O3032JO7An0m*Gpq?JOfIUsXflYRYuR z1$X@;D(^0_>^M4%E!M4+Z)5SwU3mQRQ#Q*%+8ly9xKYIHoA;L`Nt#E7j|$Jf@z^eS z;OOR%OEkyiris@XF)d8@d9k1PP~UV-xdJ)QaEa7wBn=gpI9SpEb4 zd2W-f>S20$LP=za)}ctf_fcQS)X#>qA)WiyIOGGp#Xau{2jH|Nc@i|7n$BGaL7!sH zmr%2IWO~Y4BDA~_ENt!L3#Yl|)_(UP+EMyUCD!L#vfAMedi<&0I15dAD=*@Bhx*6n zVk}Gj<2q+chtEAWW|D_d1Bd2wBOm&Eqn3qFd*=XI;-*Iv5^5S&++! z(hSy+dnA-XwatCObl|>w`)mlz_xfwHN1Gu^uB)Bw(7NCY26<+6%jczDk7*tIK7 zC+~QEAu7$DqN3IhoiNC16e<}Lrj3EV*;=Zuu21+j!{Kh4-^va|>JMPX@1$_+$}kI- z#S|IeXSHW}OltNbT7gNMf;LZ~V|KPCzoU^pf-$&o{mZQFZ3Fg!7*R=5XdW$PYTr`~Zo{bbieS6XIa4D~s{ULk; z{W6@#{2GI%JLJmW!&bidA9|9|8))0cSF3%m2iOepXz=mN1{(+2Ty=n5)v1 z7po<3DD@GZ6Ze(K58K@Cw|W3)&^*d1Nkyqpd^)gNvw_l2gHNDI#{15Oj5_7r$czuq zCf3bPBRJ4c-%@o@(5@`5QI{IY$B9| z86L-3w<|p*`A+VBDUFGd+==M~40Qp@#kKbWug@xmUMu&x=eO`o>o)1|v9pG>9XIdg z!_qJg==D?u364~zn4!;+#>{qg)zO@3mzdl#?uZYtMwBx>CV&M7urZcNL0{Dh)607= zEQI$L2#XxcSeus=FU-R&2_qxBWb~B#q{c-XxL;~K7#P`Ld#IAY}atEJ1K3Hdi z@83r#ey!=U%caF}a@1PoK*`biJ7&x>pQ1zBSv)BZPHTmJDsMQ~mV;yt6pLeF6uHv7Ljtu3kzEcfD&g@rH>}ABUsgt~__Fbm>#tn3@vDm;1IA)cCA7 z^RmHqtbdnMlO)=J zFYIRYxx(6Ur2HI5w%CQE2G3{8^@Vg*eTH$;;?z~D(mcPQM$7wNyFRN7DG9Kq38q-) z;}8h*ynndP=0W^v{u^mmx<1X(9G@y{^_U&F+}{>m*T_#a_q^&;74f>arGCct-As>5 z%SI;%>iY69G;zFNK6G4|;C`P7BA=1KnC$*ie~n+iQ>8rz?q&&E5JUF9e`KT5ZdBOgvPRd% z)aM^1MRgu$McEuaB4sd)Cth! zjp%A)6?%_Zza?ILw*4Y6exClEnC}H0WGMlKPSQUdN`Eo)^*mTGcuM$e34=c zR#|@RlA#OUmhJY>m#^tMgsv(GzP41oWLbUU^g!C14-Fk59)<{gC6AdiCrw}}E0&}^ z*_qZdRe?(tmz4_T{gxte{fe2b4WiCFoiRK5)D^lv>5Y_d{ye6TsnGH9far90$;L{$ zF$^cZf2{HrMSv%Ut!ybco2yRjKyuX0e3iMO7cduzwQF_a^IeFEhR_G(-L$?$1QEAw)mHVI)Na=G!!{Z!IWO?^iWfcWwYpeTd?9*r>d4#Cv@2lYx_dPn1^E zaP!NSAEnFiblp1g4&g57a&7F!Xvg1$b@9LV#Qn+JINT1?1Rus(AwUj28mCd11U|>J^L`kih z+NR#|tww)ksdSJPf{?|R%{TGPtteKp!yhB{f_w>ZY6MB*7d~uvq zFe-sq_guKFY+OHAVIAbIlezL}DO!Nx(+e3SYw`u|5V$}EI+*Er(wj50RPHR{rV|By zeM`bp5?S5{^cDH_MuQ$*=N`JA8X9x8thGJ2IY`|&TqcM-M)gH6385+OW4MuP zeK}clv6}fI&!@JcV^J|1qkZwsUl-m+Dm=tyW_H56{#M&_mR5O_QOM&uJM&CQ$ZRX$ zHI^hiV$E^07`+Q`5<;5;c_KO5#V|o-G$PAa(Bfeihs7cv(g?{CR~WCJ>wWVXr@E|7 za^m_Gw(|w|g$+A-J9-&i3F2v7l;GGAs|=;KjuBa|UH!5+9ylo{Jfby9-_-~(NIt_A zWHfE}%H$(u2xC(je3KaR%;;f8LfL)pbWJSUJe0E<*=-0Lv9|_pelJ*KaNUdrMw`?l-;`*#K4)`M$y&Kh|s~Qb3ij3x$B^EzRxo%lX1y0^K(Pv)4$ zfafuq&-XamltU_#%&h)D06Rd$zq;@|ND?8({e&VKxXNT=V)9JIw-M|#aTgK$iEkKp z4vQ}yOOo+(gv4tPp)+BVhj^Ex0WDJlIAG|oY*>B!ACfm6WLN==FR=2*w}Yx#EPk7l zvBoX}$a*tUt-@7C9!vmK8uJJ^{UARj&zvN_Tn3YF^NFbA6Bgr4{N8+ExPV6vpbyD_ zBCrwZvYo3B$3{N&g8?sgeQw|{EhH1FYwL+`J4U=t73L_CcdX!9hj6*d${1Njhn{ro zyZB%vHVon;1f;VGu~LXJ#iRY@t3KHC^i>OoL zpMzGP!O_K*g8?49MgUla%D$jWz}~pC4%^Q$JI zW{>+>gc4LCA2VWh$=QSD7yfXS!bT!VVwi$Srba1DciubbP*u*h$g2p4EFJoy^LC$B z`P`#xbJctTB=c870Fr|Ygkwk75adw5jP=I1{2??L1L4~(k<_(~`U{j3plTP-XEUKP ze`33f8M~YS9jE+!ef{Y<^+GNApD+K?>$}IFO@J{WPxowr<+QB$z0W@)$h;fWQcKN{9NoW^4e+T(?sPqp2ivYnlMZRl_uJq9vC&Sxu+_K zr$8}qQJ1xnV}bL;*}J)9Gr=W?>fu=Y1Qs}^6M2LUqWz&#Fbp&sKZA>hk)PVa0#RYi z94;MUu~B9d0G}~buE?d0N)Z{_tX9!En_5!02{{Ymm8eX1>$2E{35JUEX*@n$@-?>f zOot#WKaqfI-=#}jjz@U>&_9Zt4>Dvne=c!pj?V}ZAU>;*6Pn;bZZWEK$fd`D`NgLl zJuwug@?xJf_c$y2Mgt||0wk{e%eq}mKSm*bW-6BOB(9DYD@PSnAIL#I}u zpKPRct|LpvrkeD481IjLE2i&S!jH?6~AJpReb~@4qrhJhD>KFO$X| zf?+yXEqKUM!FObpZolv2F6zf*f#S4Hy6VFri67)f_FN{Pa@&5i_9PMQn4B;a7ud}a z8>9cogjmJ)ZH8n(M#jrP&j@&!=Fc|}Apo}y+IKHad?-48vGLC4;7!4S5`Q)=q_c<_ z3pO7{#ULI7w{6auoq2>7K* z;o)uU)EK>Z2Zh$+FAh9L`&PEfo0^zGa}vqxO;c-6_2P7I(a4kGY*~>Tp4Hs`0@Q;hgC%^gl#zn8Eqyn zNmp?arKP$X3X{V3!nP85MCF8Zq~!%58Ei5cxIdD1-l753UZ}POG!b+aeRTCQg@ao9 z&EkVP1HGWoLt`PH+5g@-80`8?cU2Y9{n$25*KU5PIK1_I)$9}QFBL;7NB$vqmf#SCO zo-?Ewrj3^4knpHkdf%x+EZ-4vp{MX<9VMY$@;M$Gos0Kfi zofajaAau%*Q}5If&@m2lll7!te^c)Tt33IklJLH9dkM%C<1(VqtX9dA7N zq%#tJb1BZ*i9fd#fP-h^vne@nQAtp~S1uU23b42doE@{#fe_!gvglwyjop>W*{!n2 zi7sRJw}9BJFX`ooEhC zXKButeju2A$! ze5!~?;?H08p?mSa{e_@tj^CEVS3ON^WUM4Ng?`i1b?>l_M1O>rFuokkQQ{Sj5hi8P8X~YSHsoj@*7RAF~3btJ12uEXaE4rB4^hU*{-kyS<+*e{sIJk_ z<(;m;3`mcOKx}&PjkF+S5hZ5;g8z$egs^SonV7<56H}giN$EU`j6*!kqp699|CIVq zo5{i15Ni~89e?aM@A#cb4yL3ou|q2cZN~wc?4pxtiq#Zq z=WqJAmAH;iRQ#!y$exe&Pb@Ev0p)HDR;Q#f(HGW;#EeK><=93j;@?O_)r(7Xer;1a zmSpoFC;HJ`=JNyJ|MgGh8#R5}RQL2ez&i>I`QVK}SRT)SIv!_t#n*1I%o8+ zU;nPnPID)YxaOuUsyd?YywHSNL9q4WK<>s4OT;_I-UQ4WRW#7k3)l%Mw1KfXn{k#T zyh?qrkU)$hf5B9-8V&d9rO@R{mlF_A+<)n+?UDO;(fm-^00VNMNS`=VvVp6w+|2ifK4hi(C%|MmTUTR35#&SQ(oM2(*eI=emtlJ|tO z_?gT;b7?>_VM&awsLrJn222wc&R~V7|DLSsV>@RR9M%~Ed!k9pn-BU{G7X=wk*fw| zEJx)MEHEKqieA4j^EOx9 z?--}+*d))UsiS4bHCGu|V8usHFEYqJ0Unz#zQIGr=HqhOs9IQ!g^now=&C6liu(qk zV~S{I_-gg#Cuem%V2LS`m0Wv6w4ivivy%fGlJIUDmMH9m>dXnqeqYq!;pJ2q=bW8U zIsCSc)Syd@id_1WfCSEEMvKo15<;21=c_(2z@-}#!U+_ui#Eb_n|M2qZ zAs_U}tpK)^Zk_E?WMPbQPs~~Tnd?Ux%_f0A78u8D8WZtrc60{m$c=%StZv8|)iUZB zokyH6vo$g+68O$>`Kb*Bta#3r2;^ulXnbL!baP0$kWN0c3oCEqJ8%?lf|wEgMYkL* zcK**idF0BKykU(Du%7IZ9r6LDO8gY~(S?FUzxc#G0dm5~!!HDo;K8syYOUDw@D3w- zahn8tpet(ys|^$WaY+T3#u&$qG2=}eIr}$lspGBZljk3$yb}PLf30LuHiKL$ z$Er6BAav%}GSH7u)z1PNyYM@~g^AU}aruUy&|=Rm^*t;i3Dq-D3-ovfp#inOSj5*H z`cYcEV3Bx0;0ePMd~D}^eI6|8_Qd?iau3I4mIuH&QwzQd%}1tuX!=Z z;%7X-XR-0nif<9(EZv3b`+3>1T@Qt@90P`A$_b&Nm;Irwj^guKd$5mjG&}a~n)<}% zc_L_k*$49g4Dj>|&v}}0&%zBxWqh_pp83Ty2=q}lDYrFtvNFsYUY3g}?QA@GTJJ^9 ziBTJhU|ce5)3&VL2$lKb)|eg@SID}ESB#1!)bbfMyL}up_EA5N1~Qg!;ZMdqB-eKh zKI>foU#fzLbE@Nr8`Bm|T_z8Xv49&a6Wo*5$&!!&m;?5Z7BDlx&Swe;h^2`EjwhE@ zHOMSrsOX61!5Uc)WPowfGPcctYAJf6dKS<(vW}vw>6i#^CNpOh7JU|u7Pxjoe@TV0 zI!HJW95v)pHXmY!^MpPv;bNP6+)PsRDXNc&Mqjzn*F`x-@}!qHB5GliT{avn5+A1IW{~0g~$a%Du>c5XPk^KC!& zByTZ_AsP8ve>C|$#MXpEN+vhx>6ckjA?8v1WOKt{t|a`OgKM*JZo~x}9awf_?3Pa+ z^H9--B||RAX|P2drv%{nHGPBOEzwEVO?fIkk82IefmeNz37hU;-&inT1tWRmV?%{_ zrx`+yBD-|{85hcuO>RY#?7_EI#K5pPHuOW=Cl=53xQ#n@9~k+?kNczJjSLxx6y2Tr z%1ZOjkDZAn#+5t;j*&XkNzEjscyI^FySr#k2uuNap=iK5!Ey;%%gBP0NlR$Q86T>X z4iF_WUc_qSJ(yw9k4y$)=tJ_Ejz!742U>)_l$Y6?M1f~{uxJytirOqFFZ7uN*d;IC zX;mi|k|S|?f#dsp$w|;K#C8-6YZ>^nD1%7=JW!*^!@(dol9lgW_|CVe*gqz>_Hr0kEPcAxK4^kh+wx7YHZNNs($0Skz?o%&%QiFj<{b&kLEWd7SvL}3%- zIgM-)FXgDzfJR@TFcDwmH#NkFCzn7WhbT`=D3Hi88RhdKWWz+q6&^H=gkngJ@Pj#I zj|EEIIqQjE4DxQD2NT0c<4Bzmg*gbOp5}0N^eyYtUtA&2{kpYmO?Cy6W&&|LKA7(K zCT{Fic41Ubq_ZSZme1xP7ISiT8J|4mbJv4ILECE-qfo}KY#y5v_F-!-Fn3<{w%GG` zJp?3SSpaZJG-ynSOftI2_#S@+5eL{bBp@L#925oK2~2Li$mNuWiL;`j4h)NpC*4vO zA3MgJ0DR@)$t;a!7ZdFIEDD~m;ypHS!(t=r3Q%KkKS_tgF?sUT!|X|1+LE4y>CK8{ zY31V5li&2Vl#>{b1!MCo9Jdw99SiA-* z@u;nsj2}cF=_~W5pK{sg&(9Ma(LN$sW&P|#B4VYfPQIs(yLCi;| zPD*^Xd8DQi!3LR(=$m)e%sYk0~lKToH)R4~2IWJW0_}pGnWB*Yb2Y zhL~KQl+xv`rED;`!x`UNw#$gbm+ao8$Dc?PC|3rQUXeJ7<2N)fBAtZ*Iu>SBP|`Zr zf}G9Fu44#UCYv4^kV6-53hJ955(Az^icX4SqDejgWT7(#$)bILL&<`S0e{RHzjelX z;7>kz7eW|p25iXQ6tE{8QJib?!~zKdycADT@bQ63f_*jy(eN2O|2>U;)NR9@6+5vV zJF^y8<=bR>k|)L3__In;k~yhtIDLyGwM*fe6Y4Ov$~$GkvBbmZWN9e>Cf zgYHetVkBDqMY9}C1{mWG2+5v?_u1%O+Sv!6Xc{&r{=;6n>FR{dYY?fTb zc^5}GUd@P1Cak*_A@-I1(NI{nN)WqpIh9z`GcRr920Slar7_T3CwxPfHWspMf z4uTL)A*kII4>I7igvhTKB}R2AD1E8BV1of-nEb6RN-(>>-^O9_!$Als>y09uf=t^J zPHkk#o+qCUo}4jsobA1d*sz&es-=T1lPL%fVod}0dm|H=S2NT~Z`I1kaA?2%MajwnsQx@W+xOP)m zlTYy5#72wrL!e9ym8s+pxg0#`_(dOIEgI~|)6S<8d49uNl`MP&!S{v-l+NzRft7I= zO=gIN;>{Z?#H}&#iO(zv)nx(FbZ%t)Cf}7o;>g9Q9)01X=a+!2ql4ob0P(y~N5}r- zpo()VzVTJlS4eP#H93Zc4C4=m$Xw)^h6ub>vk`Rigr|ipzL%b4{h)3z8DxGOmtwW1^a-1d z;#L2ZMJ>~@K`t@pSi!KLLKZFaQw{y{HWV^!c!-JQa?LSckvJ1FwMa7A zCBr=n-#a#WFrq)PCkIec49!O@>F5O;zw9Pr-Tdm1Xr6?^bq)|gRemuqs57zIge1Ys zbT&@qLP6U{FGeZ4XbuRixZd<7%ZHJE{P-?Uv*8kh+xUPUc(%vedDv3SFx1xdu#xGr zkd7-g;*gk~mVP%wgxMI5e1oCF5&fPNVj!H%67+zRAtr$b72DBwcd%gd%*&jifkeTj zd@@yy${&M(go<=AlZ|XVxxs<2xyLO4pUX~gaU}XV^X7~ux|OArFcERfn}OJwOc+8& z1&2iUK;ek=Njk~RC7p>9QX)xiT9TQ|?9t64BUcbeV9v0|U=ZjE%>D=~MZA{mGCIo& zXXI~WO^X*e-neadnS?ko+BGI(<0oE0-CY&QJ1(-+Vls{m>h%aTG7I;`t#oa%@N{ zM24B01uva_wnolU6G9%^sl&TTE~@pL4Mh$6%^@3>10kn4Ws8xe>CF!ekCz)9QhJ-xENK!}aW{c#G{B zOqucoz%m6pEGA%`G@KHzzS^7?5uel}@)zzkxo6j_kOYn&_EzdX>sVS8{o1yHScJ2% zq|Cr(!8Xt5n0Ru{%0`ut{JaJ{+t5{rY_f!Su;oc2x?@L{KSuGb4e`o9+aor7&5XeY zdfYR^;ma`-6c%SzNgn@wiJXgnowD-Oib$Ge=0g=YHK?SE|MBqPYAT~Cc@d~oRc_;Q8up%2PS@kW2bo%7e z=5)Ze7vA`u-8qAfekLw*T$ON{sW1E1WX%VQ^ykGa5XWa@_Vt-dZVetDa+!usaM*w{ zAVh)yx$xwmD;tgx!{T*vXi&se#5O@Lv>2pEOkUKJD8|Ko(fJcw7o5OOXP50WIq(D9 zD)P@0Su)3^FKI*X7ZW7y^NgR(Ne*YyGiV3?jHhTl)3AwTa|4KxV?$INTIg=e@8lh> ziZ?eFfu^Sx{5PG|zw&O;Y)*(s(ZZG|Uq?|pNeq~L#7_e&FF6UJ>!lF;F!jsdf{dQ3^Wz_H^zx*lge z@jR5afy~6A$teu|Bw##b2g?U6{P9s@nZo2`Lc0jFyO#U%gNt9dB!aYikxK_`$FDfM}{yPA4>0zbUcm57JlI|6VaFP>Ys@P+S(!sS1)9^jGupK;LV(g1H6us z=N>?F&Ak>`A3f6z?UXLayuU%!_daU2;O@b!X|sYuo6{Bnuo| z{J=MgeR9s!VQ9pE(AoW)zZZZHr`m%^ix(}yDQ<6QgFQ|fvsX3(H4 zuQAsm4Bru1%JW2Wj5izZP8?}rvlWEsgM z_EJxhMYA88pjkY~b1&;`3QHgkk~s8$&H0w39mb}x;QLpJmQVhQFwwoSIG~8IyYoF8 z&-!2L{9`Mk1;+5$Q=1Q!bc6C$i0J;HiQ|zG8DipR0?nuIL}jw)Lrua#OmTaG3@+*{ zI&5InixT7H8sDXlWv)h;UdYA2_(+@gvLh@t*SyiumNFYpt~NAFWe4(KdTWypRQWAq0i6pLjajgV z0hy59EQrhn@R8$V&RpTK@M2B2eczlgC1NyZZsO$~l%$2c5i`2ZsPmb}MQA7&{!ovh zlP~7##GScg1CE*eQ>>|6*QewlHlF%jZjkaS!$8qj>3@f_ieN^9bf$=qp z4oWH?bn`eV|NS=Y1S$n|B=)L*Hv3=y^}oLS_?NyF*q2uZ85kyrN|K`n1FB*<{_+o; zGVP!oKt~gjte6LslH`Jn=^{aA0QBpLZ87+$&u)d1@c1aE--DJdPb4WhVE6#&_ffYl zE=*)QrV9KizFbxSY!vw-4ozMqn*@8{Mvg)ACj_PIO$na`({!t+wvL!#r)BfdSE*#s zFqxd8gP3bo6mM*jcR~ZJ@Cvt()i`$Z zHGarTAbj%U&tt!R{hR(wR^x$Z%n|sQq&P^bBUYWn)s7&{y#Z zB28l>>+#dk7iE(3_A+)Y%Ge)%a7HF`ArK>l<11uoGlu#l#daHN+MHSA@2JB=Xa z@q}#OC1?}E%D$zB?W2dU+2lMAfWZ)ecAWO$kT0!6&qnjau#-p#Ls$9!ViKJo*=(P` z>Cf7G)+KFt=v^9l8L=T8039nz-;6BhT+=>b$n9556+74B(~fQ@z7Y}+$~LJXW1{$DT?}%DFlP;n7&s5^x>)@8 z1}t(-B+2Vn#fOo%7dHp~e*lZbQw7t~ty;*a0byL?eYEK+2B+rp(M_00-`ELol3e?ja$jwB? z7dJ@fS+Y4Q^U7)F3p{TUvU7ZDRM?DF$1o^3f-sB5vv4|)V5FNyVvPcG6no+@w=pZH zJhzoFa-N*BSViY#S-gN*o;iS@8CT}l#Ul4^UCpp7cEvygN>WgQt1P~>xo z8(2_(`E4*Idv>)vBpMsol2Vo#!p@#d*q{iQR5G+s)6%7*{#b(~@^F#lnBR<2J9*es z8(ki;<^p~`1(igO8zY|b!=pQ~+O^@xGTnB=R&hO32v zefh{lj2Yzzo4mD@FS}s#Lc?buHUfb_aX#_Z@z}5qDmF4rHaPhkzo?Nd6pR1qizY_ni!D&jLcTC%zm?ZmAx_1FMcE-{CKy*CoL%^571)2)7%6bS*ZihS zUUJ(H-LR~2=*)=pg_rNLliIl8s|)l@ktMZjn|vJwECLj%80JYnw*vBZ#EBD8Z+P+m zIx*Nk9`Gqx+clL`XZ-2nF$I14=<;4VXH#^z8+S3FS_a1+a}T2AZTw)V5&LI6QAW;o zv<~n!<6{WJa>xm#qXMt(U~pB-H$VULpZ_I&#w5E4ULwKY-RKG+Zrd?W2ql>s=T)FT zc*3e9TF4V90!k2YzRdEaVJ?BtBY8M(w^YCJR*~hYG{v11xgwqmeGI6dChzmgNu5ES z^n~3Huq=pi|DjsZ)kC0ffy{x4#4bdKjZ>kO)OPzQy~;SXH0dO1cgLU#4K{&I_2_T{;^W0 z*m1_nczYGaG-#p5EP$K9gIB;s7cvWYXXeTG8Uj>Hedu@4qsEXk)A{ZB$g@%^lA z^4E1**tn%(=ZY>HHrf<1>{OawdGd=5**@96{DiH8bE}P<3nZUNq|5xGc&tb8h5UmZ z&Ed04&4Yt)5-w{#k6_OfB0GfI+W^7b?Uf?eLu<#KbDp z0w6>uL6a3#8#2&1Ap>3%LT4hdfRYH@Rl;SMC%flB2oF#6nug5#1c!tA9zvRwb^s4}-gqi0<3=#QT`M;zn=zIoK(h{C%96c)HAqUz?E z?;mV&F4ZAiCXS*TJMI8zQRkDZA&(*UJgUN+dtSsPnnlk9#lK)eBmb|uP2l0tPyLr! ztT-oi1RrcUwKqSVJnR>jp3YZ%@>L*i*#_ewx(XYJZwDkRre4$tvf1hT1i7M0kfN!G z<&8=WmOj!Dxq>1tLiby?kjbMe{J~FOImxb1Ufwt?#342$hb^!u=rA{ZMWwzUvZ?kM z%8-mI*01p4Qpu{m))?(w1u~9EnJn`pGyx z_$%7}^FRNWmjhDJk#dm=x#oI>w>14VFdalM0v-6*&@NKsZ}tEfi?)~GNUYHKude{k z%fZ?^A0|neYAhO^%k}UB(jHNshg0r1%ehbDOvcN>4v%MOl! zbTBKq_^8B42vXjUmb{d`xHTAExC0j)@jYJL8W}3dm7+1xmqX^|?CvI?sgORtX3{}r z!gx}4V~HqE=r@z9xjbS=28VSZU)GmBVq^jOl2lx--0moAf z$|g;2GO5W)H&U6EvMI)W`N_h|XX~S@Z}{yTv9p;Yj-~MU#=OB6F9u5?d*j7`#-7bA zHggL=#_~dzS)XST{?b|TNEsWsaVIuh^-xKLt0Cqhn+T3)ULeu$pHvfHhBsP<;U zNa^a7TQ_1rWKOS<#}{op#Z+C+FihM7rxd~PQto7Z%l&wp|orp=ivW{3Ix@xA^op0R!7f>iEW1KA&Kq#9+IUm3%e~6on=N z_o{U<_prF9&BP1T8Xjw8IYao2Egcr~O>88hMS$>_C&<}Hc+-aG_liYg=#3-ZFY;1n z$cOUj2{`tEpkz~;NhUp1RC%G~Y>yB=2ve8fKlS(Vd}Z{T9vmU-2`x+sC8;#k?&gip zuXVffEHcjJ-%3PFJB*BxGO^(URXZz0L{e5~ksdZkfOS{&)M~|M<&0X!4m8 z0Qu#%3L6Oc;X07=IqWSt;&m>>j|T0y!`u#%7*ABW`+RG#>9 zQGsGGP9`K^6QH*VbriDnoYS%8Gbc3l3Dk31{VrxOmrc(6c*{a;uhnMCJ6I+=!>oOaw3P@C;kiRy4dy+{eIS zmf30$oM^siqrY}_O(4fDy0=ht^a7P!VU$hT*zm}G$fIwj!o~J{mQNmug7J=`8k6h#XM+;unGM+V{aFqcJy$^TA^)+7$;nsXLRcT03ZNKL_t&p z9JUp7U=Wy8&;FWh&Jqal@17}A|}vfAYPb>L>dMvaTcTi6CEa+=C@$k_9CTfF>HE&;7k8onq-_a|RQMk;R5$@s$lk z!Ldl31ElmTk=P>+HV8e1$|h(Oi7=^>eET7JZx$$0RAC=!`z{PPxs>By{BiHu{CCPh$FSbQ`& z;N2{Q#wDMWD*M@64XZK+%F*MS<^X4NPItW=bMEX~;P6kIkl*2v-m#egBs=kW#vTvk zdvdRi^RBsvTNLT^vB)!XSlX;j7hyfmmF=6I7T0qnZ03L$KV8O{z=dJ^-D&HO?6Gl1 zRO6;*(^N?M87*REk%}@*)8(sAM1Y8%cg?UXhc#~2QSSU{&w$y8^K2~Ce(8f$|Dlgd z^I9Cb7s-Opd&^#o zQuP0HQ#Q2OON~1gir#;8;*qu&0v;X-(QhvwX|ri~0;B;%pfR+)KkeJKct~z3K5Nj1h;546S!8ag4-+ezY-+Fe93|i+eIglzFi8IzXi$*g%ptI+K>)8;m5hs-1P;bkdipN!B=e=ol0iW z!Mbe-iaa{K8cW|2X;(=qq`;;iEY7y5d+tddny+$1!*_h>m@|AHZ7IOZ2{tZ)0qsdX z;9R~yl{3GHD&7!OR}PwQ^Yme=zyJL|E*y|BGPRM(j>bnZ_)5i_ne0iDIs5je&&;s% z(dijIA7GG#N*$-2g^nuicxVtsSd+Qb;qn`Pb~2(N{~f5=W|BzJy%+;3U^g(JUoL<3 zS$w^?qa!Xq9bVb+<}tb?ynYx)bOr_@1I)7sKX{x$M2%aF_>}?W3}Sfc*k^WQMvw)F zPs2xsS%FVB;7ve_M%c~=}Z|Ax4pBeZA^nuP|b~_*h(I9iOyu zBqKvdT}>$e?*bkO`EprUiUm)ypVcOGX?j7+8)^=Yk(<^FYuPb)QF>k;DvWJY4jZ?96qK7P?v8!dFb}5{f;WhUS9wya|Y> z2;z`(=1*6urr4(Dpf?i9@bEGmvmJ*9)3Kn!DF5}3|L{K{i3G>dkN`P$d_(hkRBVLA zZB0uzE}oS<|MP$Uze6W132X9rQxLA-Y(QJU`+i*8iD+Wz_kmkJb}$wGkf{IWoAGgx zoWFC>ZZH^zvki7#iz$&=%r*v+&nVO$2885;zek?IQrm-IAH-B9eYq*Be`lab9@8W9 zO%t2};&spNaloa?0bS*RhlKv8lf3VXL!;vfBw2Wzsks8C&_1Ud_=*LZ zCZv`i686J+7q-=OV6szy-+6)x3I{e?^AuuaX55Vvzk@P5ik}=2C%2c~WSECJsSte% z^U8L-(*%pTlN_={LL)ysh2f;(6P0XJkmXa0emFg2P~SbLB*VCo9|;SA4eS47?@gdI zyQ;(7Q(g1W)zzcATisGm8e3{1ArJ^*#AFPZnAi@niS1w;XOWoa=Qw_uvtI1A{MJj} zTX9AY@r3P=g~1pD#sV`5V?aVeLK2$iR!{2cx#p^_>i2zn-}C=}wOT@A&RXwO-T&Tu zhJE%qXYYOX*=M+oU=J*U0wOHdN*IlJt-7-uF>Z#j$xDiHoE`K9?8F`ahG%S_2qT?@ zbZsrge3eUHCZ*s1&Y(VRb;gT80eJY6p0+E5RtdlH% z5*lTr;Gr?Zu93`6O+}bFPBpPj!e0;)d4!MoHJArz-;>~S2{Xu$pY0-E{K6FZ>|0*k zz}8iFv$L~NzlN9~Y$;|`oy2H9j5Dma%D`4s)ME(I#3#TTHGttK0#;aHA(#}{0i)fa z2r8WAXb}j{x#=atYHt)Effw~1#6xPO9C~*!ve9`JrrMX|dC`x>NlGv95f5<^qv&*q zeI_>SL@N9fK+M9(yrc;lG?h4-Z&NPYhZZvh^qN{6kLjq0v}0uSsFo(k_?kB2AYD^~ z>uI#DmR3ZV@r~zYe$q}n``(0R99ucX?it!Na17yu@L)Oh(CAnQti-hw3$wK@aP5qm z_nK2j{KzbRqE3v+c>pqyRha}DjUFIukYovEk}i8YD=ZOC1CvFeQfofITMXi*T4fUP zi1CyHgCg*r1qqPI8;MpTZyLfl1XXY*$Ul5c94;wbW~&XW)ON|KF7Sybei&!Fr8jy} z(kJEyW-d8L3T!iMz*|lk+**md6_nAN_g3;I4S_VSe{ICGVWO@~ucJOCHve07D?z&R z62W_Vfiy;yOFSJXoE|c7;@->n&n2Dq)ZJZuSb#MGwVlLn+^C@@Tk}!{)IS6(b)5nh zAtC{hmHnmlR(lL8Hi3dmbV|Gt%5UOhVC~hY6w*N@O;8Y}cvucx6*?_}YyS51WNazD zYw^z5qLXE{26MCtE!x(B3-m&dA-$Nk_V5Xt_`uDc*|KCAT4id=FC*5x%sn(F>j9?_ zh^gr?77~jz;VcV}(UjsLy&1qgzI{OY@FBkWHK|(Zt6$;*tY$}`V9i51X<6h+Y~2nH zjWn7Hty6`iF#|1gFfqS*i&q6=AWB%)Bfm`>8#imsC-MMhoC++QSh=SsQLG*-QKTXw z0OFhg8am#IW#tO2Rz!3p;R9j~ZZ#~j1)hi`p#axBttkBij$Xh7Q0D4y8i1035^fN8 zMhJL|vDQ?qtavd8THI+!8WZs07kKmRN-F9$8mp?|DIAPJi5nFxdwS$Q@rfQd1s;tw z3?r9Bg0PTzgZYVH!%w)jAzQARulCsFzA*_ZUMIeM)(1`J}j!f&H z_Ox_~m%r`|G($)5fr}ADAee!>D*!XPI0D9sHdFpok*jo#L?))O!;g6Ngp?&s?161W z;74T07=(o*+{m>lxp^xz#7X9~vV4v>q=BUaKP}EtX(3RI4}PX2eO!?QmXtZG!!L3( zjkyb3Tul=pCc?`ZkElR!4`;b-aC2|`S_5g)Dw%;12ZuB%LP!PCs}hP^&OV4^kbTq( zX#+j;xB4tE@&uL7&vChZS%OTgvRH|yvGoNmnm{m_OxHvVh8B=uMYr+i2_mt4NpLlZ z^&RTD5yVGo##Q0ru*5VXZrluwD9e7VPqQ+-J|*KQN%_uDDca zO#8;}{L_eR#nyi^~J?V(F~$jqBB<4Q3=_)FpR~YNBnGRRKZlmJ53l00KnRD^H-N%C5`s zCVG0si!ej*N1bH7MkYXtydxHQHywvzf|mwjg(R?Y*Jb);$HTMK31bAlAR?_sG1H^T zf^s!lOdD@F5oZ~58M1Y24{wVmFGM9Z_?Rv=AMZ&oJS2R)q9ckr&H9TD5CN3wh(}xZ zvnXen1$8=wm40HIwHe-qH1Lzgo9coT;4On9nyGlfL;O?d_};*XM3l=kOnHP2z*Y@e z)g*|D;K#d+WkptC*>iwaNGR@2JP9(Zx3RqPmP8^Hi8+y~P@{xRHze{|<(*U^QKVpq z@g6l|v31$Y<+EVTr!LOWR|c^}P+UZc$_e4r2&*Qkxa(qShQ$M+#_}G_Pa^9qK3dTd z490Gpy*dpQDtr?+5&$zAkZ3V(HH?gEk84lR+ne8>leiibf;eq#a}C^^Y@&za*^W8l zS|$<0W&QHvdj$H!3vuk=YL8pvw^o)l(V|UE=(`sc9KxIg;u7@%oX&P6R?421gt48; zFQrm|!6VYgOOu!Be77xhmdoc9p!Q_(l7aq4I@$|ZtvZQzFdcykY5#g{V~0M(qmp}? zipmbD!Nd9hjsm0Bv2_x0NFBXsgP49^PLTzwjejhe$0O*BJK!Qbg?6@COk2lTJ&p3RmiP`Fdl!FtTKOS zIKUEx$cvx2(}?NfO(4yCK31YIv_2al8-97*kSYe^EO%bKSSI}_U)5q-*)-WMDq+O0 zxnh}d0`hmcBWl!n)D5-6y#>z)VMOW7)eQ7Y5Fuct2k|omVUwL~)>j9hNKo4|)^Ai; z627T{(}0a+F|&kU%s>$?{4lmr>Df+gdQY5*A?^_~vWq%X<|N19N4UAge>5I^5@#d` zD{0FPVqsyf3=KP{-^WVWxd@9R>L{mKq~J3rfQy*4g1_ZU`pgx%iBIAZ@gi!LML4xx zBZe?=Q3QZlmpvXWCCWj7L`X@V(%HyCh(1Ju1kldZcY9SUHW@71Dtt|ocn4EaOBt4p zU?IdOB)_#K_EuhFBZ$ONBEzU&B1i7#)iCeb>b&($nHrcjREJ~SZ5abjQ!y?Xj`?7s zEsGsEq*f9osrPaGYb!X2n!SiSB{Gn_CihDb101aXn zIRhwQJo1y|aNXwE%O6tKdE?q2S$;bE=QrHiINFmHt&z0p!yjA;x5~@O&c{vrV0F_4J~851@{1Hdj(#NC%3>Ef9)~K|5|5p zH?nFb;ld6ajDufzTZc4oTr@_|62aA(T{zO1%-^5`;)ceEk9p<}S86>Xk&6GmGGL~e zHN_wEjHCUC)3SFk;)q!QEujqyM#PG{FKPHmq|D69O9@0Qo{bw7;RA(@$^@}=ctlNN z{Gt%r1_H6jO~i0by5`|yNvw=ak(ss12Ao7xsJ;-394roh;T1n_k)^EU09!z$ztP7BR$}*Kj1{xtWcKWN zBT1&UyATdAYkfKtaLa8P+?L{_jlGRu8 zOJd2qqa47oextYH*>ve_L{ijGe8X{^DKfBxpvoh!pwqEOFP| z5@YKFoFiQ=TViAVFLTRYvqd!2nKtNETS()mnJi0Y623v6z{r&CdP#hZA|r7#AJT=O z0Htz<&uGFLtrLYX>7i77B+H$)^h&~{Vc%DkP~ z06nm;w=9X$S_U=9UeNqCoCi@_XN^(FY;3f25JXIC=t02Ys%=#m1SaLqO4!PzVtHPV|h;V zftEgqVs>Vx^s(IY zhOgvo+BizR_p!WNKhJ|x94d^$tXOvLyd>hTTD2IcH0R^m0P%OL2*p#U}OTZgmGmc6Di)&>fff+!yG@^F|=7Yx;8TeJ6W za|$8A@F2bw=N6ts45KUvq%l~UXlS{ib>CaUQFvx9EbpeLTtNraMGAqWugo~2G(V_9=!j)5DN2)bLH^i z@iIM)s)rk8L5J3eM<2MaoSa@P=WkwL?z;Q_2-~t{1NMQHY}BD=SkWK#H8+{Lna z^QQ9n6HkT5mv9INiEsUS=|bkvhDeIxg9K1&laV`aP`c>F`~WVB_!(p1MAHK(}+)lhmX1{SVGw90j%IBmzvYAmL*-@6$pBnE+G@&WW1|Ux-30nii^&d zz%}biw&<2$-}}eEi!iAelh}pIQ!QqXl`qq~GM1Y@!TSt?K&Bkw+Rb%q^Sqh+LuF)S zqzNZR$1{Pgcl}G5B%Z$M!V8#{;rCp*yQ~>oS=O!@VTIVLSS7aMyp7RbV`HlseqeK; z;d>zJ9LLg@?&k-0w_AH)rz)Sl>xq-cn;arH5=n?kUv4-_Asasg2Y!YHbNftC#!tafH5XbG zb=sb=`RF7>qiTQhO$vPcy2*T!&P){4k3gsXc+Dw910L16DLO8I$ zoOkUK)^42Tz}@znVy*rRdwRjM45zmUFUxE$atAUef8s;$B!P4Ew=Ib?UID; zw$#49VFb}qx&DUfp1dpgyWaQS_fSXps18IS?ftc0XR;Ooi5kf*LLwaTN$Wigd?KtG znPXxj+niQrFrfO9$nl;JSs`J$1WsrI5m)~X7q5YJ=5P8eMKq|DmDQ9`6UR(npIsho zk$kU}?Yca7SVgZ%nOk6y3a3t;DmUHq(Q?}@ca}p(VQRSYIeEXl4#>kW?!!d2_dmdO zA6M!Irp5 z_c+6RZXDo3y)Zpz`)y!c-%Z($9XnEbVRFA)xB{EO#*LfG zy$1Q)UJDo{!-z>+N?f5qV1t;Qoi39TQxN%7nPCVt1F;jF0!98~dTU_iSQ%uk!;XtC zDO)ygDZ8(@!eW(`*gkA)=|wsKbd)6+jBwlLBpQ}vq8(-bzSYppgQ0z`=w63VdW7N9 zV3|QUO*15#!Cs=4cK!tygze(dM;|Ij4<9MB9CRYEeuhp%Lj!3$0bUlEnK$)Sd_3PG!K{o|8b|SQz5d@m&AXZ^uhv863q5<7VH&0#6 zfs?!ww1j_Z*U*13KOVSFo(ks1FZjpasl!R)QCH@}f6JL+<7Hf+s*VC`3>rwT-xJ zow$)!_yp<~k;1oAvY8cOBA8L5%7o{8IFOEMLh&#z|5?FJ3JZ*XO_O(jaSd#bE_3Bu z5+ay2X}miIorJLd`m>)Z`}XhS^ILP}Q=d_#e?IQ-5NV!k9Aa5S2ps15@#jofRsU=E zT-})Ps`0{}xf^32XYE6*(^E4LA&d$%*CNWWI^Ua)7=q2-FZ>$Mh-;kc8f~<1pc@r_ z4L$p88DK7Ykfp~1Xg(g+>c=>p1Mfm;=v=BKSvs#`Lxh^i`yz%&iEJLhG(R&JTEYwl zrkPU+iG}Hy=Qh225C(fk%F5AIA%u4A+Es3R&Fez#zVH4S%K20|a`b4}86J7`VYG<> z;L@Wr6ghL|m1S(~t}+FD-=!nn2!iAoLoy9&Pwv|rLPXm}|B69MJ{Q9>dA7|i>@lGR*fQ*rl3s)^kURu(s;u5x6)+W2ijF-r3|4R5kq0nvh+9f+T5rMDxd!r zT%U2-?irB6q@i~Mq30%A(z$*(VOhrt%C0~EvkzpP$cabq%W{eoZ7CxJ;>fIlR0O9M zknai3XP;ETvd2mSs{SGVjX`#nEirAdsvc0exD1csp#Alf; z%weOYG$oPv6EQR&UKM0h5XOWjj9Ca($p&f9>Vv#L-jHh}{FpaEr5S@g-F0+zZ~U z8{|z#k1qrBYFMXZE`U(t<2g+a+*|sC5nOR-kIa-sFr@xQGQKW}hu)vO(S?vY;oaOA z%cIt(RzLu|*F5H{It7&Gf$9_Q2&@>t;;;5Fk3g88o-K1AUWXj$GG$ov~yUPU^Zb5tL!B{$2sf4M!001BWNkl)vnR@_o-5M=W`}BB;(3^YGyf{3qc~`lSj+(9)?T2DxVrA40+P; zIfMrAQ8a*>DAaYbP#vk(+yGg}12tlR>Bubn+vvgZ5yKv$jZ~9Oy>S;pP1R!XR&_9x za7?Qd!wTn-RtgOXOCexcD+4E53CVBB$7UMVHVCS<&b`6C=I5*UO(D7e=5KB;hYlW~ zLO2xDMzODHx|?3qMnACNl-um=WI1_kyc{|<3!$OTb{#5DW9_eQV1Q}7+10#W#dQ(a z!(82E&z|uze)vE+c<3KMv^{0V=0AYRA?)d7P9g?}m5AZTq(g z(=^M)LnHxKGJm`oA%JICIgPse z^2^G>gHNIvjFp#MdmY+UZ`rwHN6^*?!!Bt|2DIl-Csjgn)KB{On|{@15fK_+Z8Kqm ztDettue9FTWrnsvXDED!d0ijnO!nPa92naA%je(bP37A=qH#^O(0+yu?55D zV1CjX-YJ2igl5)Y*Qx{}Mh`7KT_D6y6E8g*TzC<_a6Re9ajC-?Wvt(oS6wzA!^1EL zY^9z~ZNTSg#=&2{F=RE%#{D*R~E{i&A%I}`wUZr ze6)!Uk>i}b<7DIh;ya8WzoI(#3?_4_+Q(QO7Kz@p;gps((7~9h!2zSu0Fqa$K#8D6 z(2z;E%vtWM%-95^G^t!!^B^zQopD)=8}!Rr{H;Z(kJw&&}MOT7>z2H(fu&s*5h#5#_t=lAUGCmd)jw z7hi)=a41oUy<#W@r%P%LGH!mPXQ~Dv(a93}$t2O`<_qx~D7@QA3RNao*NC+c4R2`k z==$>yHbVg6bms1}SH+|wJOG&q%o12dhE2pU^mU-vAk`@ZqE{zMR6s>GG7>@a1a>R$ z3ReVRQ?vjXVFA_3?UqSrs8BU z;q`U(+A+rBt3&ykVHkGu2{y6F8%0rL0HL$7~WX$i~ z-(9;lmMbs6s=WHf8#&O08I&M;I<QJ@PuqE+XmEMJF{^`>WZ8lmCid&6I9 z40UCo2ACL{BYe9)^5H*|-GC>!w)0J<6&ZV5#~G1vbg9N8RYwLQ)MRU5y2i`Qg6TW` zYj%-&4Oo6}RS2+UdKa#MObCG}SiqoQ_Wli=POzpoY=&{@m%i`?EX>D3w#eo>E#s=_ z^1k!e3W2y|QLl!=ex|>AHI8xjd-gq?$TJXkm>;2Fn6nT)EZtRw%TtiB-eY^v&TyPO zKmHdV674cEHHk(01o)r^)9dM651!-KNdy2YEruxM3_aushe8a7wcdC!^l9$SF{@fD z7#xGo^WN!VN~qJLc}~$$jfk=xbz0-WVaa5b)gm}UBPyQ*t8vf1TNbSI!N?r~K@Ww) zGhCx^G%8Q3J`t&b1B|eLP`?UjS+<;Eu2VwpWPUQSQujLr*%^=$FgQnqf{ST5dqMS1<}zdwdLb)AE>+^`PG?WoRC z^F)d%U?OIG9-uch*veAb={f_av0jb29ao<8i@^wh90F9S6IlnWR3BOeXhCiHkg-S@ zV1mZK@ypC*z#7m_7f+qu4G0mr0RirzcT3RcM;8SDyUZUmytf^8P~LJ~JnMuHCa8 zyVH0X8d2a6m0i0oE^mC(o9Q_6%hy5H(6t(`yr63{aG*n& z2vqNoSq-)8BY*y)1D+zm)|uYhd~(Vrs}a-`!VT z#ae$)voUW1qMuPb_tX&^H-vsmRci-#Gf)1^rp*z!?z-cSh-j&!1`ZzVV zZ?HKZP{wa$L5L1tLpUf8axst~s#{!~#iHB@ffnbh5i|mUTeU0HG;yyzIWIDL=;{&B z@zati`K8*_2a@@Xr2tqi1Us24N*ch77oWf4-s?~dVc-nOO04mVi__)k_$h?ziE}dj z_U+qa$Pj?|QGnQf)%A{CUN-N3Yq{(dzgYT)*HK4ZC5{;&LREMjE*?K|yxjKZ?=B~w zcwbHWg74Q~dojk~<7E%)+ zHnaFKg;|*U5$0`YP!oG#&?Bb~lK%+dqcNwQMDL;z*4)DgQ_QoSJ~LE~96XQme7c2w zWz3exvp!r$$gR&B`ppDff8EvPyp3DRZC`vzIWfb!gvqA?_XQZ;RftYCUc+@Em(;^F z8mu{j_?axf8o~gNaCOQ|L80(axU>z7*&T;k*`Cx!{uU~B;RoK-reF{8t?GOm0Rr?siP&$AB?C$Yn z8B8s*ZepCCwc1vy&@{Df`}Qs6#@D@}{K4=1UcC2?TwGrDQ-4_oR&U3^#t_iqU_;Vr zk0P2h%r%3MJhK1k^5yq^Us;@aDAIquAA5S;QD%?mTdK4RzxtDcAm85d)*mU?VkGQl zsx#NP1i7Eho;7K3nY^Z^5KbY$fAB0GVf2e$pBKx43&Fxm>_81aeu9yML4DB_*3<&` zE>vch%o<0)0na;8^6YcviL>tHfuDlGp4@wdBI7yc2m>C|Y6=)v+dcH)Bfwi?D(>^q z3r|B>`}ezuCi+P6#kG4s-=yizhHViF0oj`-p&Z!nYP?Q&sPl#AuK{Zd_Y;w>v+p@8 z;#se?S3b-8Tk!hCQ>V*$o31T8FI~gtA->r#u)RzkvHW)deJj_OxyHDj2TWuZI$jLZA@=ih^G7R1Wlmrs{o20Cj&{{x%IuJBHlCzv9t}fB2H%M2s6uxxS2! z4bXEAvao3+hEDeIM~<_Dk_AJ?%k%|JU|-q)hktCAW&OzdvU2SJ^ZOI!^wamCL9y@$ z?jIc86yXxDG(C0l0G$*#v-jhSo2s&3%s9>O_AS7|ZZk0rqpR#G+(7L~);H+dL%tNo zjc3GbOeo=XoFOYY#8_UB%0J%q*Hypvo3mZ`8XpXpv` z{F>X>zaKcTk@feR%Hi?P6ZxvL2BTxw%BQglpAnJwnVE4!4_lZK0^t@^0bqTqaG-~|{(fvD z(gH3Q5J5v6#3IpYcbJN4zX z58SQVEY5Rq{Q@&b^42^yCKRo5`sAT9*mazZQR~4}H9l<>8g;8-*B-rP>ach?%coEn z6knUMI-i^P3JL2;8ue4(523=d;d-jsWRKn!I2lBwMgYZoMh`nvQ7AO^n|ic-Xs_;WgWyPFS0Ny8 z60T5XC15ysQa?pTt`pGYn{EaC9j=3S)xI5u$NH8!hF35I7$t`Tp~zTcapm|7H?fi? z#vCxbtLK!q5#L^(E2k&U#I)B23G z-xUzXNm9xfHN4(yy6@gy?CuUEAGr5UI~|Nfe*&SnPT(JcyBfe_T<3E+BzP5<^^Z_^ z;Dg+spR?YSm*!T>9`Bu%Ls)O`1rQ5FKR8h?-@Li(-hD|qMvwaX*SsOlcX0239e^t6 z6t)I~ zMB{q6%P7nnbL$v?*J1EHHhh!~Rc;CzE=QR;I0f##V<`ycFvR`IBX=_tNc+gY|64y= zrs@56pYFm4{B(K$2j3s`GB&oE!&}y|YV<@oaG+7?VSbIUF_RI!xDugp&0}Z>F-+0l z1R1VZl%ZAE`C$Ej_3k^F7JPN`5bqAXbj&!|JbIgWQ5;ga8!4`xrGO_m{Se|_^u%3WBn zAAInQMEDJ^Z=b6K{CskFA#tA#UMGzDtZ?b?U(2etqd_#)|Jh(XAOGvFzPLQL4?D)p z)u@_xA_#iR@YvPmzWeTjDYpQEu_{Ka!Z50o|M+}BW}MgjY$>1ly(T{6ZG^~V`B*Fc z7RIwfL#V_9m$0_s`w;M0#rr={Hf>s0wr}4a^YXU$eH?IdiggWBY$|fh?ZT`P#^5r= z!9$!i(9pd6p{tCWZ{MC@r)n>`jCXxYa32LJT37ngtW>5rE$9^JbPN!#?I9XnzsWqywJ0jF8etx#K-j44nf zJALb{lY(X%e{H1>of=7djym`?QXS;_uOX1%{I<82*T42P%T`d@NrxsN6E_{YdFfIx zQ;yNGr!b&h8@mrR*4nehVJKxM?R{Gq zlIBl$mpkvgtK4reS8YE$b*h~A7^^h< z*{n3(z$Bh*>t{t~@*{N%1f{SD0_1SbX7P#&D>K}HA3XqZV&{HBSePaR@OBW{5*5+D_{9?x%ht25D~6uI0sW1X2ACCSHL`L%gbN>it^?+ zzd15;D=EquyTL99#^1N^sqjBe?`Vzv=l}d)%AfqnpM>An`?>zQ%ge)$?G1)c0uh<^ zGGSRsf~$tCuLsZYWOfO(7R<7!!sQZ~P$lzrl^o#76`ZfXgz8qf4C zflAu?tbcV;pV2<@66rx_dT<3DeJd~8_=t2_%; z5o4U+=F+`}>oZ)}aw*7P!u27pQr`7*KV4q>l9#kQ*Wf#t4954-fMi8D5gzoL7XC$; z;kY)B@>0|Dko?{Dn}^3SAq#|gd3yI)AttbvFv#?2(M8IDgnZ9`lc9Tx=gWKln0eQ! zs~CTrVixKqVC^eM4quDZrFR*AaoM{c0%8Hr3vn*f_3(>xxl1p-m{oYEP%{s;7=Lnd zjOUAJlslItX%pcxZ(c^q`RAX{SZ|iGAq_?opNohq^PG3ymh$RXzpnh;JKtHhY`p-L z4|_3$({|Y`_BIz@C=oyk+^w%)`?X&yx8Hty>@jc0Z$G`|%{P`uAAYjzJ@9yW_+FU5 zC4s7Sd@{%=Q)u|AjNFE?{ArZBP)kV0oMA;=h>;efX8J~UM$#zziWir1@nvO%wG@-9 zpDaCFHh+^56O0LT$d7XKgPE#K#T7NdqUj02dh^=BIh7rJp5AA$q`RIg+;jp_pmk4{xb#85U zZ4DYaEp0E!>2l}2_aVqQ=>|1(dRoc&?C!dk zUW`F!s9CQshKyG*awv>`{O#{%r`@6Q%fI{p4UxUcu3cA^16{ky$U#r!SMmwx1?}YI^#kbA$ zwQ}mcmL==XHycgEla_E7-Bd2Q{<@3V6R=RmF(_FJpZ(nJK@Ku(Cv(@Sv4q?zMG$Fc zSu#9-hsr-G3%!=}eUw+iY|+-T@<+h3pX)&Q#QMv@$<3~(j@zH~(&u8T7n)YfD3n%^4;moykXe?xxtSrOpjrqW{U zBBg_xXQNdg+vcavOq8zo{t;&aI<6pL7!kNTKd7Z&Ai|U7^NkmP#TWzTKRI!x{HKpE zm9_NvOHV7=nHq}E9z!(U2YL5QIycF1)*cypki!5HZkFkxIjqAEKk+0RV;pKEg64MA z%CBLm zWqEMZ+wH#ma!z*1dEqy_;SDif7rtxwfA9z2Tz2f-87KSx)^GiGl>gRSzF2ZbH9@uE}#{^!1F4ujnw1Kz+BHW0zP^;<=89nBQAXMY=Vf$;YP|k zfOnkd4>vyFrrUZ1*!ZpAFmP!ok5Mn>x~u;&>oERD89(9D@Cx#0c-X>aTT6_eOXarvSC)afaTs$Aob@?amXQT%23VQ3f?aaGoc_|+kID?edhn(v*m{7Y zYqC9MWfcoIT&q9GTK#+Oy0wh6ac6#xgD)Ag4YJOE%^G@t7MdU?$d>ueW*v7|S3heG z)|D=Hi>>ZI#zz-mf%)~NXW^016xOXJi9-RNzS^ z!|AD0C(0?dv(C(LZr&-*EZ~?dPwTPWs{pVXbuzfBXSJF)>Ocna>j<(J&R^;IfX#9{ zHv^7cazmMT;#SViJHXW1AjJC9gnx!hg4@7#GuQKQD=sAZk1`ykw+ge^?{WVrE~IVw zDD5y=uDxb^`Pu__GdnO;4(uC7@IDdsMTF%pr9#VZr2KtKN3VD>3;@pMzW;}x7FV_6 zeJ?$zp9EKe=jP2@FzTw_{}MdBxnbr7-VydOG~gEQT4)6>I&(j7o#p#QVC}}sUS5X3 zjcXkj>mo-s&}M(k$;#`?^)Gu_dE485vf z{JcDoah=9@NJNu}v@#$U9ViG;FzEHT!}Jope%@W?Th}L>#*aWq(U08y!KknGV}l$< zNkfxX=5~9mAE=0Jj(Em-bM)9AQYmw}zkUSf>}A?yZ1t+xW%vC*@WY`l=U#iZ=pr!O zKI`eWhsO_bgx1M2e&i&~wg>!nga%P|uPu{zJXSVr{A_uk=kJtphV*aW`~*nPl;8N+ zy7KZ(PqN3LiS zz6+h~^%PjMEO2p-+@r%(O$^t0SE%^vAs4##w%B|RPk(6dSRIy+DGZqN&z>G@Jc2>_ zt8)^j4>fggWPpuKv*pC2x1!M~PzR=>(So;B5CcJoNvs%LACcEL*lStB7>M z($D6AQg+Oja=(X*9xvCwd^a1d&Mz;2{SDOjbb0uJ2guPh(=vyYX7%yLv$wXXW! z>e|X5ww01r;5Ygpdv4;ADi3hUZ2EOFU9d2Ix=#P}`Fg*Rf*;G{sZCq9j3D^lUmiLx zGt*$}5tNTVbru*pi!(37@0DEtflC|^7_?}agdk(8PvLAkspY-|9(by}{)gXM-twk5 zl~pWS(teb#GQVZ4&pg4jXFJVg%AIgEYIB#ZtAJ>yGcXg#%RNd1)nWbO!y6HK|HY#P`S7Si=6SV}v)q3paEK~%QIMjJJVx-VgIVFkVR1kANS zPxnQph}eKL%?C4>*;rgW5u1%-J+;FW!e(J?k5W3#iL#5paeJLFSk2Uso?p&*T$I1ZO5k-&wHvmx=$iLFJGPm;1!2Cff{FvwZ&12=h zha_9asH0p6fRXQ~JtVZdxpdpP4iotjXz(>WU&EDJUYVhtZYL}>hYu*zS!;BPpK*ll zHRWsfezm;)$95wKSs;VZe@^tVf#6!msiQm%~7Cx+j_4V6I~F2+wj zj16-EfuD{jvTO9;=eQevnbtr=c`=0o%YflO4n0lgZ%Gj{jMYZ~<2L0p%-@!pNaulGMA_ zA^I)YK`AnzMs`s;{; zfu)O2KcAUn%*UKBr`yiWemXvr6rV{8#&;SN)@^JLOITMOFS%wS4mgGDayw}!^ULyT zEZkJn6$0E`_U`KfxOjpM%h2${DCnjdf>>o55j=UQN(#k^&eE%+@tqD10D;+@snNgt zN*Mnh?q+tnY-Bm>3_aE3`yS<8NwR}_QNpy_AbZW55j@x7zlUotmjwGbmqaVqD+HR} zbnd_Nd^L^tTDCKPtUU7Yc)8|9Tflj#tY*ot>q4%)`jWEui6@BnT<%lP75E%s;(@J$ z#W`E|{7AXvX<1?9|*5F`~bH2iPN>`BVeJ;+&&2g-ea^XuivgMUoAZ(|d>OS+h~BA1po{=ggJ({RCv z(7eoRCoCU1#Fgo)P01Ic;L_+<4VOg79JuAm+vc_L&bPK-4iDh5uvMkM++nVK8<5B5 zE#bNsUnESBmtTVkWTZ$QvHyh>k&&N|bI$svzxoWT#P}RCGX_J0E3mR(8U4@p9lK(j zKRZuPzZxL`lbty;z)b2TWsL2elXE>~9n(^dKb9c)o|UNUXalF|{m*nkZmZzt<&U}3a?OVB{Y`pAEWzQ>rv8)){KnKxP zMyUerV@t?N8Dj;)ym!qu=oBG2a$XsF%Ws!2%$+V1PiXx7`gwdk%=cWpo%%#&@6gy3 z1lHG<^?%CQ17}0nhsVB{En34T)pT$8{@0f)cV8|;VTFQR)sJl`ThH&tfHzZiT+G;))lEIjA}q0#^UN7ktPmJz6OO~d*OP*r4-F=!hhM_@ zHV?5bF5ZDZy?XTQ^rZIcocnK2Cetej9p5Y5tRUA$EO0Q%JZY9t`B%}PCs{^1I@r$# zE>^{-u{KR9czZ# z#BJ^9`m$x)PG(KOgYU+hzMti_WE65bGSFHGGbj)Sm|;m<3m;)c_-);?6?jAB^;tgS z#Z=?54IH3yxO5$U43o%Xj1Dd(;Zt0Hm+LmJ2f1_#t_!$c#r3YT?jnq_58Tf4fmn3( zvddmo_U(P5T(#$=Wr3LTvz2`F$fB%i3)>l&fy|M`dwC)@7t|wiyOAf*@=zJZl@6n;YPBZSCQ}o9N*$W9CW2 zvl!dcE4<_qNcAk2yd&;a5Ts8ExSZ;<)YbQ_Kbvm&C8<8tE$!^e*X!6a_O7qEJN;**~&|MPtx^!1x_BR@oX)Yo7p zq-Xz*GPuAu?3T_eFTS{^JTgNeF(6KI`peGgFO(JZf_wJtWUK7U%WZ%4RZeob1zh{M zCJ;(=5$sDn!$&eTboBG=p%GK`Up)nN=v{4j<+s^znHQ3&{9 zN1an^r|I=35IRR$-RC^2*6;@&WKI4eS^!#&!qZ`b0#d>0l?yYv%FL>5yC2mOJByCkAd><|I;VR(+m^NcFFjYOzCor*U&#L=Uw$r5S-m*{RIo<(K|j^p1%8^ z#F-Emzv5RJ5^X9M?s#=s7+3=)v;%|~9UjCPqzCZWYHVL6O&Z8uCtWLhG0AXD`_tvC zANt`kf7CJg^LOhn|K&0=^M?#wYqzd&Qj4f}6#}B%9nJzRpD!OY`nPHlb6Bx=@ z#_6$own2;dkGz;Z3!)sqt=xY=#p5dfbe4gxc@ne7OWRb}n28_QdFKT+QO?tfJN`Mt zso@S;X*y7za&IBeiv1agJ=7m*A}ksZ4^wJt8O|s%yf+-i3gGQFAMOHb97H>m;P0 zahK)9EAL(~v_ex}%{&r*GQTy4t4RSv^i1>KKl~k@QF*OVJX3mQWVa2rn7<=&1>`z_ zTWGoMO`t^V{olDj`G^0)X=n2iQ5-#~KGuHCDrM>9SL)YB?@?nP50$ zd}kDT;jV)R572`yln3s)3$2KwQpdZ9<0r|wSj2Ry13NVWoq}R_=^5p_d~2UBYx?(* zehERdy4-f_zbNakyrI1GRfFZX{^?V(J(u%YIe1k<n_kD2aFwV{wHV)q#~<#OcI}vS(;WYXB3kx6b-dj3A*Rw+{Zd&x*>om^ zZ(&+`4WCe-@FeDD*cJR6KLH|5AiO7YzX{L3L>h5!|JENUOHX|qX)_zb!6dO}1LpUB z!PZ^n-9P`ol}#KPo9cH1FBqgjuzI!bGLm7>ya)3KXEu>*v{u@Vr1~}CCQ$~Y(=%Or zNKk#e!_hyLi_HzUi=0;J?>18TrU;;JEpA1RNs>g`v4_5bGkZqGdqOc>vr zzt6_JXZu&-P)m_QqQ0F&<7fvhf7<>IjPLJR?)MG8xmV$(SHG0QIL1N{_(4n$B#f=7wL+RaR}zYgDeYf_R_np_7OJF z%wTlfw(TMq{&czIvMuZiJW^hC^))P>8jbTD7x5qJ$nak=dz(F^uZzV|+b*tQc$2*50ElB zmYwCu_1O~+*&dEH6j)g*M>ls=beX`0Ee=f?v-1k*5&Ru^j#T?$|hB`ND(1KlQ24 zmYY8DssAN~z{tqe^!9gJ_~&$I3xj8moEii=#JQuafCRT+&mNK6(OYjMj^peXHeNj! zU>B0OJ$d(~m-DsT(Xtwgb05M$0pJpB1%MiWy?;H{+*t}g-JP}%pwgBx^pl8rRx6AX zp3wB1t#D}KrZEXeBdQwU-25~QNI27{4yUw(oWja0Q?Fh%99zN%hx@Uc@YP&rEQnXS zHWPO%ng+wEYhF`M_p@^H;bW{ZV1X8koVqs-mNU~&bJ**dGWDnbjm0>s!(OR(R2{^T zpmwMXllfJc(M;ylPIMs#;wczEo?v4+SslOX^ULm+VJ!U#R+kQe=U(El-XsjAAK|^9 zR()L=Sa8@AHYEwbQ;>I;68?c%uxblkJk4NeW;ND^ZkQ?;F5 z&wvqtGmgw{xO`dfGJn`5f?0{z;8US$q;@?E3xP6`?CtXzEi4wMCUAfS)C^Y{V?5@3 z1d>=;OziT8*T06PjMtUFxas4J>Av#6lpvU#WEI@=aA)`S&z@vG3gU|(lutHM&JqMm z5bC3!Bh0#VPV8LP>K8s_a7=&K-S=}I!`|rqFWR*W)psS+tyjbn>s6zDK_N+WFuG$- zuV`p)Etdq5L&6mBG0x`4W!NiF#kG&P&LYl!WW?#&kFiF=c})BVV6JY6>lDjhHI_Ps z%G?dpt_TkV!~FcC2$l!X9wEYyeU7jd%(m=gk=Lt&*+NeXn^VZ85fF_Br=|nWJHEq0vKN`Sqfg24ihB(KC`A#(A>RWpSg+Odn z(6i#ebEFgsms4eb@s}Sjx7~Kf_n;s+AG{%^GlwiH?iaZ1f#2)Px5TBUa5kv3guvOs z-^O=8R_SoS8IK*?x0lWITC3J$7~H_(Du_)Bz0*xHwQU-LA==a*H2^N!H4k@%VVxFL z3xJXBL7YZiz;@vB>Z$?w1*bWB;nPfax?kP>5exI48q{7_v81-ZP(s#@MO8e^vHs)C zs-30l#MO9u#@+7$(>3%snZ4=y&8K+`Z-W&Kt(WE@n#ea>|R#tMF|(VKff z=Er*MayOvJglF@hhs^H31+ow!Q8H5E=yM0I5yX!ggvN|X&V#$@6QATru!ouVKk;1_ z2KJ~w%OG?!3t>Q-%`q2@yszer(YMVT5B&I zY0sV2bNUA3wqj@?m|Wzd zpK&xEH4#;5ea3aOym*u`>DzwvtwBH&Ov`-YlbQi%dA5SY^xR}w@tF^m-Ya*Pd+&Gvu3KDoNXf>H=fOyQ zp|*Dsc82xpw&?#ty}gU4+*=V^1A;+q zq1qGj&J2KEzds5BpAaa??y~;rSkUC0ei$kHhm+3G-WHj>iFyJs?!)VJ~Vr6MSf9|Do~n@WYRlsdY!n=nB5@GdsZt8&&6B50P}axcH|$ z>-X$#a?-_Aw@2?P$+u?Cnj~F;;PUhhIxs19+FT0RmJ8!%!=QMka7cd2I8CPTK6b2Tc4i%!S5PlVBQ7XnD z%#UhQPNN|b2Y*kE^?2|H&R1(P^(4F~&KTx=6SB4fWl=b9*&^X% z!`M(+0ToCED)>}U93o6{RJ5w(SMR!;gFN;#z4REMmY9M%#Uhm#PLzqzH~RbS>3L=L z*S>ZSYxU{za2m^Wj*U-42+q6PYfA8otTfauvf-s|mCBW1pTF<9g-Rq`_Ve`wk>QqE zxZnD^4pB?lHJo-%omRtp=9!CahKJXdu4{gp_5Dwk^$*>_Hv{-4f@ds3N1oyjF5VdH2xUd)lT#ZsD&|3q~Yec+;V%8rM7-P`yd}>P`Yc`*4_6& zY79Q>H-Yf@PPq}jEHg4P9N9S~W*H>7V<)CsC|t~3{EJ??8|LNYBI4N4_Es`^a5ph! zcF%BYWB`mm7b~__&`Ua9*MmB{f>nJsrSX_qqvso9r{WZ9uyx#1KKz;cT6w8$EzRlr#fRDDo0U&k!ziHFz@{9lb zFPEJ=cgPK`{DTh~I#17}`F78*ukq7gUaP$K%;(PG_wr|XF~E>G)>a5HU?ncuO*a~Q z&CaZG!Qb57=+^at_x&*k*brSEw3dVCd5AfjcsafAVK zqdbK6P@P!TVps{&)3fCi<2;3w@K(aiBg0HnY4tvbGyN5pUW8hDhA}U_6av66&FEqW zA&kqYn1v^NcaCx8)C>oqQdP`|N1PSiZV+mE^X%}$-yb&*xklf4<32uS*-e)?GCW2e zD+y)yAg6$gjEr*7$TS8z%1iMl$!B&-K?pFX?eKfwN59c^1E~ISSjcAX3Q>mv-!10) zA@Xjo*yH8-xlV!5`TYD+e-p9ZLfX&LJ_+KruX%O(F^=zwDRC%5a!;2`m)7@&^Bmu+ zqKB&yq0Zo`#=C=RI|W3B8W-OtlVEfM7jNVI&pv-jOEX(?4Po7z=?u{GmSu zo>Hhmtid_P6b%$nS3t;^S~F!_d+cz=_cBtX6%ELh@G$;p9fsl0*yC#q3K$re$g^|0 zLLuOnenW0VWl?DXlw07<@PY9B-k^r16i{JZC5m}l;51J9n(L|$a{|x<58lH`z87(@ z#r@^g-*+SRw#0#->sgXI$%h<961*@>@1OZI2kIut?2%R1*}ca>rZcRipW?Gu-rXf= zFTd1MlT&(|06n+m2@l;Jr2SR77AMRJv@W2o~acWrZaLdP1wr9D}JQGc34Df0)tn5 zCNn#=7TEeOsW8Z-(Zk!L_YQFS5zI_$Q4bnkID`a2GRwE}`UW^01OZ^!0&CTK zhdBNVVc`*JeZ8pY6r{(UeXQ#@tp`%fG0ip&4rizjZ73i7#GP5_XZ%*eJv*RtC~$Vz zcjNtSWa?t9-~4W5{yg%$m^cq{y&VBCU4G%`w=lnlpfb4eDm}OI4rwF_nU^lL1K-k58$hfCY%%~h zGZ&G_goJfiP|+5q@Qf4t<(ugq55tTeU)3GI+RKZiW-KknBw@Hf4pfNFu+#{&iZX8- zu!&zYBaQ|~hbO`GFjoi!)5@U4ON+62w3**!uHnp&d=DOayzJVwyBy_HO;=oTWf|Z! zAO()Q%d9lZy24YDa8WV9bupxIJUVQaonq7Uz!Os^%M5GeW#A~53BXA`C=SMmV!^m9 zI&ok6s!={t36UtJJqdY^vjH8xg!a^haAcf6!$~vV)fgP-ceAfTEx!jL;F#abGI&3u zjex+gGf>}_T7mQCSF)Ie=bJt+k#@RsX#j@T-oG>MxA?gm;j?(I{2hXHPQ7mDeRY%h z-{Jm&=J|W+mi79^8}E+~j~*Xq^AXzM2*SFbLu~ul4z0lMg6=%ss+(okE(~(Ng|ip7 znKZAWkW9)$!)M9tS=Kh=Lwp^sGQYIxvzLNDjPZ@1(ZdhWco8Q^Dxl=Me68cx@TS8O zX6rl;d%HXxs7jVd5kL0W3WNA~2)(?EKV)977`vnH%P_`KIQXw?Zum>Q<|>96;_5;V z26Yd=o=z7=VViJm1`av;Da|a*v$O~k2p{h3WyjbO{&lxvj)PQ*xGcR8f%Ev2`^(z( zYs+EI0O%iBRmMC3w9EW(b+&QC`9KDlfr~L z8@-nUJl%{lFtn2CD^JcHjMZM|ljr^ovOZ!gt^f4q-AEzl$~DH!Oiv(BT->5_tvUF~ z@MeSy;m0t^Lspl%F*c%Mxu1Ip?F4yHUb$yI!hwcoTJF}z&&7NJ=G*_X{oSQ(JC6g2 z9|viz@*isQWOGf~$+eryd3QAz;$j4kI>FK{v2;Fp(XQLsAzr zb=e;HOGCY`JmfB2y4p`V@#Lr;hIyV&t#B~Rj!~NSo>W^aAh@XU%TBop0{H}HAtEV! z4%c*myar7}GQkE7&=ej<&-iZjuC(6c7}}mBBv6yUEMrv>VfI)#*Q!d-vleO!tO^>; zFZ;y5NHmg$j6_&w^g3YS)Bpe=07*naRG!EOYKdU{nk<=CY^(aDWHaw!He;AhhV!$} zE-rHKwd3-rF2d4rC%%m0INfI%>CpIid2;{WvUB@|Ebru?(gBWS;`nhTa%dDHts_4F zyisc~*c=VX7Zcc+!m2mM_NP70i$=C(G>Qlip?;4cMRW>H>vYbOg={r4a~PoCG{=prolKo% zDRT%&W*TNO>M?Y<{&IRIi16k+J9}HQcrG_`BW44qAfL;&@LT|zut_v9>aDzbBL^5X z6aNw942Igrja-|!c5!`g_4pcy-BjfXPkS!j6pU&Q-@`#PLj%}u=+QkMZGgjM++;;_ zMqAkKwJqfr4~#KHnod^c73V=MYN&QH-s6aT?!xjYI6F5rJ~wBDYtmOrmoK!H1-SrO zvI@FsNZGgWOO^;tR=)j0-A3stadGztJ?Sj>)-KWY$J1;3~)^CJ=*mQz0aNWA=*;UCL5x5UXuL&W*OO}Co~M5orF*gDHi-h7tFH}U#qdiXVW0pOl<0CDey zT9_^R-M2@K3=eT;-z8D|FTd_3sP=q~iuSXE8)jvpy?zgxYWCjAoYGJ9?vx=J+_L2K zur%v=l4=HiXr$V$?AR0xUF>nl=FawZW+>J*03n3}HX!oPvm3~d{Dm$zx0#>Upogr* zAQQngei6O(Ts!7AeHpG(Gw<=F5y+7jjA2;o<78ZV`rOdrnr=RHL17kyXavF)nWY2@ zuoO->_HZ%I0a+P3dVZKYzDMp|;%6tJ%n_1~yXif&LS*cPWfomzk&iG^s!B0_Zv3LO zQ8P6rM9rgbQ>g2Q4m?%P<8X=xA9|d+8{*{Ltr+m8AbtdzsxxIZGs_i+T%t;!I)0MR zIv=6IdtfTR3&59*28PQC1cMBx%BOSclhee5N!146{pg+j2}7qbVPMOtvxYTFst$^w z20hb>YqERJio`2FarT9R!DmvN6*Uw1MjQYrtl;txK7KpF$jU~Pl)8i-;f*$fCUP1r z;YZ$Zb(!Z7({83I@4ojT)?Cm_K{+#=uQ<*34Zbz*M@reLrUI^C-UY}nHDGV#zKUxv z*C8&AeolQl{#Psb&biU}=*uWgf2Qo*vAw)#&t*)-4uOC?;4qlZ<}`L2kBWowhdGtl zuTR-4hek`=53-6e9Rx@nO5ZVj#?#JC8WWbZptdQkhxSaHYj~5VG?;5EizU-dCnPz~ z%8&4ghq%c63WI*M5SKWIEsZ!yH}IGblQ;qk%H2nv2`3n}9#0R8NjLJ8+1?G6_@nZPFVf+_DLz@w-BK>lqg!$M-TsxYl<5->mIFswCqQMq z`i5MtDb95)9X1Of$5>wr{{)9m96nSgSco*k;S%!p0LmM+S;z z5yml&&i(FA^$oMWpr1MYA?Ef+hDJFFXC<=$%hw!;uH32`ST!3}yLPeQ;DW2cyzLJLs!1}Wu?Ll90o4f}w}^z+y-)mPUYEMa@;fv{YJH8XL% z-160jf{?O~T(-?AIJ2o8nf`gZ-Zex8NzQZ3)rRLvIfmyb*Eh4U>0EG^`;BoxiHCDS zO6AHcu3&T1s`%cdUvplCzx3?Bm)PMb%`435!FyHtvEgXEb;{P=lnV3c&{|nY>%34@ z8Qu11?V*u&x$F?nK>O0JydZCwmpy<2Tp{7CnMcz*%+?kpFUoTQH=RuH$xeNZCSqr) z4M726-qjHm7e67~;!#>jf@`87lqwlDIP}=lhan2Y+~FnjJBAh!od5)00m(pK7S@2w zQi4nMD0+5~(;{k*CqsK6g(uc7K#;0Ms^9ttOc6}%q5ISzLYg&@@yT%F)m|`h5fAW+ z3RBu<19+dEnJR~lPGP-YD7!A&8WpC*HC+l2)HQI>ntKY%`w6V^$2bM{1ZxIBk&Ce~ z_1n!|vqA_1R?aPQG{h|7XS<`M@9iJKqKY=d7<_Og+i&@>>FDS>3`nbEU3(urN)i%{ zhrJ-wOyb+{p)P_?6iDW$eiZ^CtU5C@j9ReH&rnY5l7T`PQx`eHN{3hKDKNax;Ws~- zXYO@7EedWdUK$vLF}lb2dNW^e9@$*pvT<|NrL!)c-aLmedEhIbD+gFF^nw=REgZ{C zs$;<&E(uA{vw^l0xZ6G?Lp zzud&sa~gjb3}5{1$Xql=r@}BW9DXu+Y%n6P=sZYkTp3upb@5j8#F>Iso$vrcIKGAj z^DlOLU~C@cF0VLT)C~^Flp38myafJ>ja0ylAvm-hLkD0lkVp4ycbT?~z;%dFhYI=S zEoefC&|;_Tu%pujPkBX0*3C0zWY=Sl3~wP~{+`NlgJNYPv%dT#H+1PC*$*#w|JhJ<8Vo?sfCepgzp;;^d5jg*@4WIpdTn* z!#@16H5;NlqkIC|db02D?L`G$K`$Vll$WoyTH!%p0l5B9SJs(!8n`zEn^iXlr)!Il z;pg}O>5_F8X+W|mgz{w`yM;c9WhnW31fI;$w2#VDlv+g+h$dMzVVrz3b+pS$a3Jqy5QFF}B zX6KHrWej7TN4@)9Mj2@ZG}n)P$R*VS^xi!fkfp;g)RES4w$^tUCuv%yqvlxcX(zQ9 zC;T)jIrVNwp+>CWSUS@iHXo0UkB*8SB$s%n7T3}WXq!;UVN`LB;c(S2(T+HVCD42D0=RWS|EHmqIRu9G31`wQpLcmt_$>$H z2Y=Me&f*p0_1Mr__ARiCl8jDD*@{ZaaSYD@$&MPeBnn}HKwO6RNNOtAbLOI?j2SG z2WWnNght^LWV3us6vDT+S4)`T#2N*_0*s$t^%d5+3V`x-KGEiHi#F5vGON5yg>0{4;$DI2<(9e`9!a!zU zSY#%GwsmPTv(ijcrCVh9&u2_S=urYZ5qkWdV1bg*pUqiD1)eU-&wXeN&4+r}F|xj_ z+r5Fj*&PTY_F!v>Pg;X2ln`wytv$Z?0Cjy{dHXxw!R*Xz`5MQIZ`*c$x$pMR)1F|# z{~%1g63uA|oIS`y#?}Vo@HFi=PMvv@v)|LBN2Yy)!R;Mk^cXI}WI7qgp^^+d4?)>m zhd;y@8lgfp=w61FX5|4XF&BY#NgvR;I~avshr0@nJ`L2ym5$j0EQ1fg#0rHR!^cWvfyYoK zW*RzqjB?0FCe?MpUzSZL|MV!efR!;CWU4w{rCT6t3JU@>1%O0bEu%8O>Xl&@Q^8S- zYgQrMXaU;u*rV(&mBB~{bx{b%X5d*~(l5e}|Huo6NW5$7gPwF^JG zao&d2eAs~xcC6eUX^$K~L@zlL3wjoT)6bO7=;{q+^}0=H2~!ZY6*mBpFa~6}GR#!Z z9K;#ZoZvZwz?eaJOfy%HsgT_41Ys6Bg<$^HvRjxxo@J8BIKCnx`-3omUCS){2NuTn zx4bJubu!lg%ZIHWcfWOmS04@Km|kPw3VJOU1?iTw7RDn)!XchiN%YLAtuSr`N0%d~ zLkKin3vWVUIaRp=;S~m6;BUofsz_~%Xj7&TTw)(S;wSQ4yMe<)5KI!>%p~7oK!~Xs z3=WLO_TAXr1hde4pkX}?&fVkFhuX)g?*p^UmG7Z};>}&q|L3 zN)%VqmMsFZB^&U+?2iKc#WwW8umBOZCC~zB0bD$qp=UU~-oE?d=lT62s_NWn5z2Gk zw=yFmBO)UsBbUmm7kp);Grt6uC4~LK4?n8&z{5$|v#_k3CS2-no!!5?f2_q}Vz;$B zU4?AtH;24Yh@;eKCc+;TfX~6B^rzulm(_(PVpAN=!fyU4%`v(|+9@mC{ z!Ow>4kzFJ87FruEm8TQ@tN;B^Z$7yHG7tUubK1`s3BB|1=$=0U;(OdYrNyiTm~`Bf zaf8O0k5effg3F?N%mkkEGgwc*{xS!Fo_PMBBe?FfJ!j_dYNq|S&)m=2mgz87jPRyi zBG`2r@6w^8DBzhnOxjtsj)Uu2rppX*4YwQxNcTK4oTW>r=*9Xl4vH-hg1hXc{}K2f ze#plz|Ww0Z26I! zWORN<9&M-7(nsl(Zl-!G?JIsrB)_mkr?P$j*MIrH-2C+S{_)LU|K(q?|L`GqXP({s z(SQ9}zQ~lb6v*-#Wj}lNYvkc2q2rp|0CQ8!PN)OQI|`8}x5&J8tl>M8 z7(>h`kKJytUfQBnX`r6qzP*DzX?V;gS`=BqvHniey^Ev0xFhdW-6e;P&-yw`b8_1~ zED6KIjt)8PTD_E=I+c5A_u3re;Y%|844UaHyOBe&%+7IAaTcKhkbh^(cUcxVDEelm zN6un&OyevgOC&gD*%=5&&DlJH+5UXBm3G6dQ=9RK@iN{;$6KQ8J9w)5AlivM0%_+| zTdQWPw3RWg;>5|+Oa^>13n0(FCOh}j4#BvT_bOU<(r&RXy}qqWoe=Zq?E z838;v^ur(drq*{c6nDO}DM>jG;<$mLfOh`7EG2wnt-qE2`DeTcl5+!3>EOb?@m142 zN$7g_DV@v_f2aD_j^A0p*22!eaj!cQScf2N5rIt za^8SbjgR)-yLH~-VW_#d-L`oZu1&dtyN-@m;1-9P-Dj7aZrz+W;; z``QCeOo^T5KE_$m@R?qFSn9hUe>bC@Gd|zm8$)O0wN35d57=zWR6n+5=ha?%WzdXi zX?2c(=4gWsDDGR?5B4=@xm}0tJ2TDnUdQ9|jXsl^2y`8-Ly+rs%rbCx>t92z$d^37 zj<_K8twh*6o8>OMHT=S+NX1OCIuvOLv=|WN)L|M@C$4W<0y-nHUYVXGmnikMKiPkA z{7#*u)G3C$6mr5CTTIL!)hbr)H-ba1GeSSOV6GmrW_2iLVM{6G#`jU6ZfBNZfF)Dq<=<`Q; zq*nv?mDG+%Hgp=;Jq2b5K3wDJzZaB$&UDt>h|h2&8vBzk`m3;Z{0-L*xgB$zLZ@UqfdAbsV?zc6M+&2Fvk#baZ~#*PlH85z}i%SvvfOJWc4P+XD>oL(V9;T=LC4 zZrpj&Q3L<#^Iu2jUvj45tIs||C)|?b-)`$>=aVn~pPL{27@Cyxwcd}tb;)f@r?oaf z_YKe!Z}H7bOK`HDKzz>a(mRaKPkA517oYvj&BsjBoh4vNaYEiQ`}}Wy#_#(*WEY;- zb8#E)yWjcr<|D2jKKcj?yLeQMt=R}FK<4kjb31Xm?g)Z z-O%(lE;EhKKjT&`Ckf#9F`KE@!xMVBrOB;2XKvYe!`AHVIs}8Gv$mX#vDmMjl#DUG z+`G5)@DWejzt2xP-{ltXJ?-PAJLNbTanPuGZVa;?*<*OJ+YL!7DJMSGH zzs*Jn3>D}Y{pANN!pgof_o^tCr}BE`r`+0m>drmW$uIc9iZ9%`XIlP(wRukS!PAFt zK4R2)Nc^*>kFvx3JvxRX*jG4;hd2UvPhW6v&s&9-b;s3FQ}5}(mJQL_33Don9cDi6 ztwDAb;KinyqqE?-ZhRfW-W3pjbq?3~-<7BN+VQCoG1l?kphA!b+{b^&n*IaU#t$Fz zc(Efd9{wGA|B#1M)RFrC9;2;}(>8hRa|vHF4d*uGFMs(N&kB5zj{o#)9z=TSa|t+9 z9OKLHJk99&@h2YA>9o>W!z(tyUa+Kbnwgh+zVu-j&OqQizx3!aGG@;KC)s%c9T#U~ z9n{_Te#~h^-u3^<$KTISY5BvdzxVt9EkF4B^P7MD@BZ@U7k~8&+LL$p{eNHGeE+xq z9vhY1mVU{*5FqBUYPTzVBGP5R2h8j~q&_ZP9NF?jp3aL|z+KO@VsrioN;(7Ox}oj0 zbJOi)cG`s55=V*6pKnLnBS_QF z2BV?ZexjYrPkVmpTa~>dgi&UBi&Hpw%W>pOx0S}EK?gy?7NIw zaR5vyeLp5+1P%=Zf{r4O`Lb$%^XA6@)YebHp(k))$eGg(Q}0)AIPu0#?i&u?y#4^= z1iohp-ghAUereXf9zc?>E0wz(MSes_FgS~O$!6vkzvc%t?t1i>kr$Xhmu@rXj)H4b zJ~{?xUO9|Krm}zk2fSg3$;YeQZ~pM+*=v936n)?*eCL2miYIK=-Q~wioY{Ie#+ij@ z0=#|d@nWl`49bTJa?3Q1Dh}&e*5{u(A|sQBqweP+7-x?dGr!jS>e&% z#etlk{nf8>H|7VQ{FE#4uRIjP+J}&XSS)q4G52qD0*{fkj*o|;ut`tSy>x?^+jVYC z=Nt=gJGRRfEj^BiG}L`oK;|mvB|rR4C3>Vr)LLzp^8i8hmUI0 z>f3xrVR@1WaW@lFwrm@P28~8O^dloW#i_cRpV?{d+;F?kr8v&VzGnYRoh*9^*2(`s z8^x~e{MIQh3G68&TO6G!t7EvId(T8vYL6gdsCpob_B9bzZOC z*K)sP>CzwG5R=|%GZ7p$4dy+B00Sian8MOAxt(&uJ3He5z3*?QzF~v&>(8BG5a*L= zHMmqg{5=hMpXvB(Hb*ntlen~Sc;@+c_u;3hn;qP@^lE@k!|&6X@*}3w1J^$C1$!%R zzI@4Z8&7WjfFJ6#4SxG~fB)vM|NP(azJ7G0d^qVZu`Ao^Yi50)|C$*9FZ2BF4}Q$E z`JC$e?)SMp_}!ei{NO`=@tL;sj)6`u4`A6Lrz0wEy7FAO}$Kzct zM{MahuF=U^Bt96pDeJY!fTzwuPGY@Uj89haGH`&Vxw&g9$5+E^U z3<8@-d^Dkuaw&=#w7&YjuD}#h*K~1D=t-?IDkvg8SGNy z1s?nbul#zN2W_01w}U@o+N$!O`>5;~xA&4eUGJ1V;B<1Q=ndcf&sb+NP3ARoH{Y8l z^IV^L%jg3}^#=^K8kP5Ky=CHUHyxl3WH%bKwnf?QduSL=0X0%XbU-~bYUuERtf6(x z5OxN2^n;)LG_EpA{csnB2>RsqoCb%}ip<1w+Rcu`p|G3_p)OP{b|MR+wB?(dfArgb|K?x(&;K&7 z*!uLxAK(0%eV5;Zhq^Uru-*#E)nyPeee>Bb`7NcdZ$AI!FK{Y6r$MLBW7lkCe#~L3 zAN=sg{QUS2ZyvK$_=r5_Ys^BpZAaTGw}QB5<+Rx}U8VxTD(%hHr&Ih{`Pazp64#O0 zSAl z-cGsK`E6+498PdgjNru_74G_j&%RPbvQfK zH2M=Zs9c{vWdrUTHuYXIJwE~?HiHh8?cp1ZL8FB|73{RwLC9(Q$KU%AFKD2#+01fn z=#ORIWh(B294y z#&(L&F#N?|{OsoM|KyX{mh$Xb2B~%b1&K1@P+sieHD@vy7`$2FZNbOL^wnp-;EsW_ z0yZ9LgNOD7_PZPyxtruu zf}T#@j+$w??Zn^PXB@9eKfQmcCi@;ZX}a*7&?fs1V|T+pfc1}H%LTJ3l|i@ zRD#a!Z3=fL-*J8E6oyJ64REGI&b4(A(>X$ibpGIz4He2ct^S7Ax5Iz+`7d)i@5$v= z;F8&nj$R4O2>9;!JLm1O&ByLAGu8Zv>m$7WV;aahs1V;L?>ftU2KN&(HPkCor|5uc zTUV8srHRd8Sw5Lck#?k5b;LQ)KmE-OWR9P)(x*uHN+a z7=bjrT_b&li%bWV%Ts6Ge$+7YU>3BL#;KmL8WJN3P$(tOGYo3ZrAC9&zvCyw+mT;= z_<{pf%&@@oB&0J7H&SUj+P~}jlw+eYRdbd=6@7wGJohgg1)YU`?B*g%Cgn*-UyeEh zq^EqShyVZ}07*naREk9A0bdshdqhD z8odOM*-4Vt14n>^sxu&o!(uFaqBHRR)!r*^73ne9RJkt(@-`a3<#RC-u=Wi%d`+ZOVWY6 z%}hprJfioi^HbU2JHqY(4$q3~I(giCbTlNySnpM_b>M;0XKt zIdx=x?-5*=O$;r;`TDERko$3NRjP3H>Je7o31EHP7ruf`MkohdD8N~|C4`s z^XI?#`OUxiH$MxmcW<`yI|$g>BT91Z|0z?I%Pt?tpguy!aVPYnO>Z(yK6T-%i;0uICW4&Qze>H zLp!hW?cCy=VWF0(9kx?5LxrxqoVlbuxN5&S3qhWEl;X1#p_TS1^R*3~g9F?Sswt#b z%8^S6r1BjRmE4XZ38zulP)}$D?IbXx!`CS=bwvi35Kq~tvP!RBN<+om;~p|6(stM!k%!>oH{qg8s}L)JAXqjJIbF zbQ~VXeuds%zk7c3r+@nM@cj|Ltza9m8aTpse*5+Kq~w`42y=NzlMuWu%?ttA?y%!& zU9|%@=^kjyh+@}p=)wY%rbU4W$L?{!2%B{DW(=K4rJD0fQ1=9B3%|oK?p1BS<|Jx; zhA)n%IOXdE>~6Frdde03>yR?|U~^{xhc=f(m!WO{4#sqNM*3{jUpqba8fg^BP?5P7 zkXgsbT*(&=+4#dzak1m?Faa=#2(L3y2TqG^NZ-R3<&{Rh6*K{q3_w$%M{n7{!+<=E z_F+0x@m%+O#OR$35N8T8mRC1F`U$tt5Y*1>G|df-9OzJZXnW|zH}O2?ez&KwG|rd4 zxrfouDVRwjJJkz3x?X%dpb3wuGBY*M-IzK+&X#y=pCamPr;|YIa5A<5*z`d50vZ7Lf+1#Y^A5H^&ess*_F<$%jq5FONDe;dFcr9Cuk4E-*Ljm zTc&mhUw7u(_ULoi68DryL3X1wZ4ZIs0kno{paiT;kaa zD4w%~hmUx>@^i;VR-ir{H+orLB;eIcT^FM4mF`-FbA-Oxc0C;pj-?$v2zPgFH8%(792{ zIdhm>iYgi`JAu<>|I&H5Ze2xC?8w1au#|y-BTpUD@iBC-c`(RzJ2Lv0f;6go3Lo*x zEgjR-=ij?|@yRzgzfB!=DAr&4-|)JwEHh|4*J^rsXC7`&=D{bfA94o4w+}h<@ljSE zP}-S@JgZ{HdT9D8Fkd3-a}6(dxuwu>i$lit9glt9+Zt(St`~JCI#ox7w;#W``S{g$ zvk&m}6Q7FwI?D*B_bEfYGSWwv=zum9o*5So!Ltn3(V3RNSKzMqecmRK*7rD00 zbXXbn*%5aSA$08Fopjv%$v^$+&HwSQf1YyyfATv$WR+(eKyliyhiZ2QKyZ1m`OaY_ zg}4bCe2y>&*6wbbt3UOijzCGU?T*fmRPFGWjk2QU`|R}Ejh%1s$7XD3^{WlXuCzT$ z#(4n9z$0Ilq#cBi&Gw+f+rH|$1C^Z{3&ZZR#6;Jxc>PrBfc(Ixde8 z+aTV4SsjS3T+Vo$wUjR~LN*-jm^eH;PL4BE{^f(0HxC&hI|Go{y>vD=sC@rl921>t zU*yvJ$CcGPGHONRilLR2BBU*KM82Um?ehf@J5`Mr_n}0y; zFaEvDfF6&|?;+6X-TbqYr^xPUS_%@2Ad4MOfPQXLckf?mOY%p(xhyT!egg)aJ`sqslDiYv=bX8 zdP5dDp25BsqOlpXbk25Zd~x$E0nU>c8FZ8-?e4zaJU~Z&W*8w&+!>E`iamG^fM7@5 zHG^qG#!!0P-B=F#qM&%_lz?y=n#4_Bf(Bk=_MAN%e~u^^bvLfWR?~!uN>k6LLiC1q zBzM4d1U4$#DZbwn&1?h@X(Ohgk-_7y-sV%ZM>FEvNt93e;f-7=`_5ki;!cG360J6H z?@m|~H>ez;eD^>`{c#+2-iFHRVI4;~-y4^a58le~-S2&xgCI|F1kOx+aK`ue=j5Gq zdGm~pW;fg74Cq8Y*6A`v9XMsyQKe4kK{IXdgfO%EJ7I@{UZ%Xr4pCBHrdJvRm|d(XOQOmwVlFxej1Pu_87 zr^Xn)EynhJM)aH`7;R~+*~lY%Z!a=v(GOxtx?sQFpI}>@TktLKzq#cwzH{z{7pK-`WFS4Qr7~(`(5~y$684HcA z+`(21)z3Mq+eJ1U7Aq`A_Tu|GbU~r&x6v8rOeIIb zvJ1|LsQ0jNGr+WRN(;bCH~-p+GR;&-g_@6*@Gp&E9zmmWlOvrUV{;vpxRwIH7;?%~ z8J0~5@BXxw6qP~oY;1!HKZ2|TbEO;Q>Skue+tHa*YjoC^7<%sIQcp+u7&@C}I**Wt zu4^(+!}*TAyf9LkfvqllpMWggWAL?7&z;g|^wc3T9d@=shwnZ}o4#v0#Dq^~9*)4s z^OQqTFPWbA7VBMZ-*NYZQQCF5cR}(Si}V&{eNHEKO|SF(kfXESMpVBZWcd!~BdmaW z`5c&P@V2h|C@*<(Ft-I?ur}ueWu7Xt_oBiaFXn1pJ>+A^sLl+Zf!`aJj5_}u1-4UafEh7G)IcOycDW`V#i2BmVOo4usDcYnX($w0D!|Vh zJ_;Io;F&s{fEk0AolX=UXAqCs>16mgl<{wartyu_z{V*MKC<}MAOQnc`Og2qfFQia z;JW;dXW*&(eWtwLRZ#wy{EE-3SFKAojK-VOknZYc$)d3#N9!eN<#ZFRcNKieH zNW_|%PW^_pu*!E|!V`YGQ``Db5Sh&T>dRj-(lZ+4JRG^bZTg)1@!3S9qdw;;NYCY; z&mVitxcB^Z?LPMGSagpKvR_e2mlOtS2v7LpgdCjnnzT!2cGQ-pdhwR5*QfV&T$e*y z%>JL*smXJ6&x2Et-#ZxMnwKu-{^92NA`+K)IFv8x@P>cJ$p1^?-+abTgYzs$pS^ck zu8*=#WTuNIG_A+WgjNU{^HO6@0rcEv>mn^#&gd+qDxr_JvS%o(B^bjsrGMSm{L8H;w18E+&* zD*B7GEPez5hNVf=`uMqxC{lxblAok?y8C2BZMBdTUP{T&JN4T=jt& znd}q<-$s=+j~$hK^K#26h|tl9X|!C&pmRq6MYWh1K#K5kEGFmGOWO@D_` zI7<$?m?Aobx>iJGD6)KPYOl?=dlUGwGeYqAseLz_^UhoQ`?zavivJ5{Rnoo^6GhhQD<-NNhUymt!+U_ep zmR0ob_PaxGr)MdUCuG{WJJQ?19q~J2w}t4q+iCZ9nfJ%(*yb0fG!$zc!_{#JH;B6q zBORah_XD0zv=8MK7VH?l63knf{!){Nq;!)$ZD{%h`y+Y0*p3B< z-~C5_c=Nygt3SQ@dq4aKH~;w`|CiBs-oHnA51`quI@u@%-89!w53RG zv|9I@rturOa6ICIrOjzuxNxq}gNHMSI(f^qU3+p>dtk;!dF?5%*lTU(U`i*FmGp8* zi1qbdp2E@@$$dR9X${TFrJ-b!e>-u5Qlvwt76av?3axZi={NNO*^Wsp@=C$- zD%#z6(9z0d(p2iywA>knCX*VDLkhQ+cDsTO|Z$Vju zDzuaA=qxNojK1@@FX>@zPIRMQ-5hkY)km>iQfY5GB(gwbMj=f<@G&0e&9x5M&d;n; z&pVwrbzw%L9=t;oXD~~{E*<32Tiki2J4TkxW1NU55nnMSy9fRQ?0Gl2z4a%*1g|!{ zH8v^`Ijy6EX$&E-qTKbJhg)CD`Glyw!5Or6T~zkAsGozT#u%E(k+k?+0V)OcZ08W* zn`rGNc9cj%Q42fuw_qd0&2>7#f1CsHo(~{HhByK5Atw~M@zyZ{=sguJQz9D*-cG;u zwC+wj6?~2}(0lrfxkn*$i*OpSWEE7Rt$6PmEGz1j>@u}#@_LYh;+W~O1Vw}>MZ z5xR6rC#9`jn`>w8o1}Bw*@4*(>6B^|T}%6HZtc3WQ}2}3ZrVANMMrS|K6ViMZ=Z+( z#Y|EFr{$U+CwAAF70-J(NMW;dRru2Zae&k0^cQ4NCfeg}20fi0&X{_*F({orKCLLw zKLj1waw%V@|8t-()52cX)Iw$ug9bov-wPMRF$`Gf_#`umtBh z-x^hbHLr-Mb5JHh@fI)vlzXx$v=WhEOMwIN<>%D16|0J9an`%$ZFhZ4J=MP(C^PPD2 z9?d!p+AdA*FsdCJbvgperIIxH@<)ao>Ocnf47^A05oc#Cd7L+Va1xaFWhcJBzo!IY zPQ9HuI9rOOIwI?4Zz6OlrA%~AhH>wGXW9ub4NkR*Bh1HJUvr2jGNM<{W9V4aqi^TY za$a!nAK~CBb;{vv?|JJCLU#Q7bW~-2$flmB6!Sn>WGp{B*qcwFOI@6q+1Xq|x}oLE zPL5r1V+Wn&JJ@pET%gQ53$Wt98HO$y#85ycXaN>K^b*Ao z$gC$4p$!v7Gm1ab3*rGCb@c50DKhR=(-4^rq$08yjwOL?;g|+~g%UNoOxs5Jc@BWG z_nt9o(Lqdy)Z4oGn#+SQHk$6*pMkcn{>`lq^;^=7$lr=807UB1Et%PK6rMFY+3;|?iAd0yE832`^P;0 z?-bbg|D(`$c;xrhS3dlrQ_cfEbly)hfP__Kcnv>Qo(){Uc02)|Z%D_DP}G&qpJ-}q z&6Nol<PMmdc=6h3A{iIE-{}2*d4{lBF z;W6Ln^)WD~)z5k3stxtvHK)FG3@AcnsF+*c>LQZ$$mk1BO-2@sJdCP3gz(cT5Y4^= zjbsP5k!-NDu8a!c(Db>k$AtUIElR}@9cAe>PN(YFs!$C@SRU{I*T%OYvX-}{37L|L z-gdFrl{vfl8p?cgOmQ%pRGcP<`7&(5Ae zXY{n;<)5w`|qv7 zJWN9e%aK@ceMbbo3cX_+3a3g`8vnb`R4ws6dG{P4qS(q}-IZMp>MZZ`CSI3k*ASgZ z8hH&Z?u-Z9;n#kAsgC-}z=8btU7OM$736$4FGZ0ff>a(rmQjb&YA6rm@~N$+cDJ*G zR$R5t($^`HW^h4nk=x%(U$l+i#%L;eS6Tu#ZHIIE#Cz49K4Lhop|b90D(KT`*|c#A zZDXq_D}IaLCPt7+#%ajsP{;@eXh$9oj07DS4vExOQW`VLoD6v{PNvkzVR{uWKV=oq za=YFNOw)joPb7K_xPhlJ1rbyvHyCUA7&SYmGiSM1ShSQ8VWT(&0N8py1$UY7bn+#jq{o$jvD{-FTJ-$n84; zdpM}KTDxb^fd=ug17#*%?K+7{9ZaSdh(x^o!A#-`uL3s^)k%owa_5zL&<3byXL~xV z+Jk!#X^$z3+Zoz$8|Ms0Li^#wow3MR+9OkPY(T?Y>R^zTw5Z9Nya))QbghJSN)XQ| zA7S@%CLW}ERHuPM37oT=q-ehm1{c^ymv}e~9q45(Y9mutfn;%)`}}rHN1b;M*%(7< z8cQ4i2Bb3g$ZSSijoMD9Vpb91t3bj`$e?nY%(5mUfxW_Z!eC^MwIYqEM<|sV*us~j zO9|sUJ(OCefY4Td8j`$R<7*r*S&Kh^&XgIMPbQXEF3_kX)N4$qG9hA7ba%sV;1a+Uz)chE`ZOF$>F=6REV*~C%++j3*bBCD_ zarvhI>lkedKCsBlCbJft&0}Vk4lFr@gI#4qm%2n>AlruOJSCM~n%2NC!d$Fi)0B~i zGPauAiA*QQOytVq9u%m{H;*_%ggQ6hc*|hXz+`S7l6b_N+cnm|HqhCa&ea2VcXCTv z_6oyCI*CI;+nR8Q^M1PCODPvskq%nY?IxfCC?|i4W;lpO_Ey+wRr8s?)B=jU-z%U( zLd)9|R%}CPP8}2^({k&jBY5D{XrAfU5KwG8eh141vDbXv@0d$VnOKFG@cj{ULt!dg`nXOPnLzkXH$XA&(u#CLKJG$#s z(E+1#*3>#U)}UEB0B?XOR&Har<^%R3igIXd$YLk)*J2`@b)C9L#vqF>y{I4rOe6M*#h7(4q8ByX^=#3215vwwQge#xGA3 z4Z9|bynus7Rt-Hx`lS*nw{S~`%2dqFG4qwfk;=~R(|;6hr$Ua5Fl>}%mzfbMqeKkX z*^lY^;7+gYbatrDnsx@Gf?Q|XF;^btaD8n@bK}zWxsK?~YaT%2E{Zz~S)Xf-36VS% zP<&H55htY?jR1-nza=c~(#{=xy_1f_#MIB0R@dva4&q=Kf*Q7^7os-uB<&>Gd{s}7 zz0bWl>*&wVxoP(x8^&xv=60!dCqGjGiOUz~z{qc5|FWm#=AP?lbSOWUoc>jq6)IV= zFYjK6XRt%q*-X`3tRXZeFKsCzA-DICPluEPKiVO>cDAH5&P>csYF#1Nc1x1d;Sjo( z*HN19$2RL-g)XOz`#g;@R7&cpZdmf{%S)Y=ZEmMZB!S5{fo$uRN*yrEpz{<27!s8? z8GbEp0M|s2i`JSA0G5i&fTsx;l zor&bP1R3=|;T`qLs-wy+E$VJ?B;OG$9XZD7jKEQ9M|zPvdr`fHjA$?k?}*7)^pmgL zOU}##gZJ%1j+pA=fw!61N4di-w{BuG%^pV(9l@t`TJ_8xoNlTxjh1&abQXY~Y%Kn# zDyg%wsg?fpo9zsO$}o7f%%s)tb{t22awd;9B%NiCI48I|K{siBGx&H^+TU^9+p1Q< zws8r82H6aBCdM5c%GswoL&YZ0wNB<%C7OaPi2O%JVaTl$+&Wxk7o0q<`DI>gCk{6w zzw3YZv)s(?GBz`n(Sc<*6hwXDrJAgVbhFpu+nW6KXVNxB=Y17NISr|ggDq)$oBx?a zOg^J$5&7;O0U7+qS)?III14LS9m7eE1e@W~b{9DK6bKHu8#f)}1mN^BA+(&fn90ZV z82p-1Zaa~uGIr{njyOC*CvgiGXr~p|wOy2$J3SIm=Hf`h`>fq?LXl)=bT)!UZIrj~ zI2SLTTIU*ddo?eNl~Q`kyF8lJ5lZiG=k*63e2mtS{JR{^u~-|n6qHRK@#yGh4b5x$ z4nxxEcqFfDJB=!GWOg+YNQb&e`jvF3NO@i{H407o&x@r=o){ zRWvYn*rnsd&ey%&TpNx4EKU@C@6TUHa%GswQ>O~+Z2b55kpfb#N%Q*Ylk#O&#w!q5H?gMtrGeZUJ&m@=Vv zmQ372v0dFLtgPWe)T=qQpU76BqPyUd8#HB2U9}rJzK%8>-+d}?X%7rJY_p@DQ>;8B zRx%YN79jVuAr(%3MCg~;fkpq#9oviwC%0(wl-34Cb^sB=wh;Z%xt>shHpHJh79I_2 zn6mOd$QUtw045GB9Sh8?NeAyJxalD+JfPv9-Pj|~oHOVgj^JoSScMkT2D1a0at)|) zoXYI9A>4q^2oA}3WGiFv0*b7{ZqWB+<6s3fh~$rZ3(Q~i@#n@|a(GyVUx^FR8r9TQ z%n-+DlrvtlJTftoxJaL=&4TAJ>Wys9W3eGOK4mu-qG$HcV$!jz&HlgA+Uvch;Bc^X;#L0cXo#i*yCw|W|SJ*QRhj*>K1M+Fpk(7eb+;VK42 zk8&3+@S(2pw$vym@TGG$Vj7-ls6S%j@332$lMXS8tntVvx{B){rHq!aQKRG}bYTKh z57(pP4#*@}sS!Bz0A<-AqtqV&WnGRm%oScAf5&$ayr3`e!>8_~Q-9MsxxD!(ODK2e zyJ^V8A)~}gP7JzV(K7)&zRbZYmo0gPI*!HJj-%fw47pdpl~76Q3|1GN<+aFQr&{Vx z-Cu)cTF$yK8*vKH=;(*xo?U*%t*N_s>O=Q(7pKO}m!XFiv2HsfFT9u|x0Ih?M^1>y zqqBvPu_LFtbq^t%(NKPiU;jfC8V_csFN&lp_zJ(UKI7vL*gTZ7cx_OuGY_b zIn&MZYZWXV+N?`RbU(#ochKuT&Hz&%;#@a6Ib5cm<#>R*%3f@)3VaaQGJsTyG=5|E ze{fPvI))@lYeCI8^S%{l%58T%$`IMk-+crSGdr+=cMm$ktYNrLs{YT8U!mqZe9lTo znZxWFUp3lk{E4Tp?fA!ieF~_oG>}))d?bB4bYsc6EhgDAAaxQ3rses>JrK>a7~mCr zx&xnqPa7<8hv_ty9Hqe}=^|)!-rN7^BQ^xk4`;fHuu0 zXtLZCWk$kp@%cM<-P>UK0`0aJ6shk!I;Nd9iv{%Kl1DxGmqTE5uC1xEFpgkuz^*VBm^F>wYDKvBoeZtl1~P+~6b8MwB7%dh%6 zgUDK!v;KJpA`ZkO*UFS5wOliWm816J$9TT`75PW+Z3_*4^2A(ZXl2t*dHH+eQ=*;; zuLy*X31ljdog9aB-#Zx`z{(vBmMeR- zc{h&3+0HNi66Fesn6cLgF_%qlXFu?!>_xOdIs=$YFGm3)Hs(1cU!)}gtL3&?TqE#Y zgSj|+6?aK2Uea5#*-W?&4}P1f1g_yqSoo$dOn}z*fYmz$FK@pEzXmCOJ6vc$#_8TS z{4BYfJV#ivve|HfWfVYn?#$|Xg2tDIF!rD7q=A5Uhgw2xR)N&+Ur93koz4{VU*}~b zi3>sJ2yJZA7-rjVU@{L=HhVnfhoQcFg|XoTtXmGtymCrUCx^DTdE;;~bVny=Td!Wa zamuWLJr74wja_}^m0Zxw%RBOOQ%W_-iK_PX%Bt}s=XG;Hivx4YtYBp-0` z#UKXPoqZUD#^=z@MeGvYddYIaa^I`qMKgZDFF>6{JBY{_1AI3avrP3+XljD2se>Th zImsF=xzX9?*8sdz;N!UuANodNIzFSTSKnyVHLd3=I>q(2mIs4$KALelg41IjnSIN< z177nq+iQ+Hzw}h6j=`A$SC<%>Ay96Y49+lo(y#`moX90XM{Q;>PJcUBqb*2V2VX5` zTj&Vg-1Jdg@3}vE!~O=>+-{{q+0oLWB}-hr$FKy00 zmKv7Ix*jC&x9dD~3@KC!x4-GIhIDk|aBDj8M4Pt0?xVQzhZ9gY>MG7z=So_6vtO0w z(=LrzJYQ5ywecmyN&ql*#)3k+kl+Q&*aS%`B3B-Us_<62glDQ0+$DV}xzWGlddR#u zBfH7S9JsX4Ac~_;-l&o)nKw8VN|dXJqX03!J&0u3>(-;!Lxv&WdZ+ngSibi@qY8YY zWbia7@$PV|EFm3_7%)4G726R&wl<6DV6PbONhF?J#oenN-&xnYnI994&LmOkOIsOI z1|#z-AL(@dIdTJB+9)@8qTJwxuzv*C$RHMC%5$|t1R9r zYR98)DuHlOBSqnX$3xQUcod~wTT|P`j_*-jXE%aupf5YvSJWl`> zHzT#h=+pXafTqC(i25<*wSgF;%4)B9lj~>$J$$)Ak0)_q#=WV>VbuSUZvxoy>5zaOj{I;4XJVNTikTh?}?Jq*BEd9;oFiR7t4} z>g-E8@@&MC@EU$)m9l;b3-88v$`>uEzd_8DmUIjf)HQUXmrw@QE*m`b}BL9LzL@1ejhMvPaMI+2b}@*`*I9L%ONc~$HH+F&}rB502qWq zOI+o$=BX4}odF0VY#o~dSA{uspL}Shd>nxIWXI;maRSKgwpISnv9rr(H)Ho|c6*3T zqr7x-WJt$1l@Z^L?^>TL9bX3^kL>#ED6Ka-Pu=oO!Su<>6nRI7t&cUQwS8Tlf8}sUB4&q%vwfrFql4hgmY+Jp{3uFJx%tUjXuM*GX;PY zQV#WD5e{WjcoCzD99NyZ0+u3;KqEWLnOYu3=Sa1ke@NxBRUNjsV7KrNDGth$UE>fM zqlsKlTlv81Xv{$&jPouJrznSS`E{zDd)-XkAMlKZ8)CxVak@xar@>jDaaxpB)^sN0 zR6tf)f0JWg=0(~Z`920N?3GKcvA=W{ml3fmI~1sV61qk>8rNmT26^Fnlh>I_J9&9p z|40TK5JhCj0 zogvj(j1JLIYLaDe>rk1Mb7V#UaSpiYRw|-h50j15^j()QEf>cHz7N~Y9zZSPunQtb z{942+^MLUrN}=)gRnB8Q>oe*#NY4q5SmhRF)5+I6S)X`;%+RW z4z6qi_lt~I7~%_F(vi)ca5e)OhJI6k2wN_B0>C-qrU_?S0s`hs{xPzvC&oCqEf8IR zmgy?;Xec2>Sq;OEmWGzp1hurG8mrNa^qXZ(j(GG{Wt2Y1!52je{R8u2Z`%Vhb6r zOhXFz@+pQ$jRk(qoKA5vO2Y@4iu>z28nZz;MeCK=tdH!oQ6zFil%aJsju4oyHLNzm z=1^$GnvrJfy!V?I)2qhc0^0JF4MZ6=H;BO~Q#yr0uZP^^UPJUffYd|zry^68IH0_J z=UPF~GGg(@A=l+^-?BMH$I8Z-PJo$!x7zyTn-0R;Wn3M#bR=P;&aC+D5D_k*!&%Tt zw(}>Os@~EwYO#N%=b7htJg{4MGupH-x|TNV)#|scm3Q@%4#!%?->0<0eyh|7PIrzF z`Pp{uSjtS_4Vg(%2ixAZNk=DbzDvZ+Y)Cuvk&b26MP;xB8&3=w3js(*om<=afG@td znSB>G^;0%=F9Upc-oV+94o+8%qgfeQBJRcra?9I zmGDw-N2}$ym4t>h=(kji?WJ@sRF299i!axi-q$xQ-cSf^zrtp@fFRC_dBwTHHoj6! zsnmO`Arb3qr+J_6nGQEOE~`vGoj!+VXxP!H0T;0|WnOfxA7C~U!Hq+bRvcSNit6&u z2Z0i(aJX7=%Gg$u5d(I&$T!$%Fb{D8*(9Zna?e~kIE3uBGd%WGoDRc11V&8orS>-W zJRKNHTm2i;3E1&cy)r%G>mYQD?dUU0O6O;k2fo2-uQS+d^0c!-%leq)E-}Je+Q|8q zSE%Q;3+gJPbamo#K!xrBs7%=%>5)0*(N*r>b4|xbkk+a78Z%E_q1@W9egPaX^3;iS zlJl)jY&psV7p0k({LmX*;)bK#l`wF1Z9Nmb^1p$EUsBm- zRfgXzu-^2Lkx0Y>b+ND24^U)+co8${!4A;OpwMC3>6U(>mDTw2IN+;GuEU0`>URym zmb3G%?swr|d=59!$H>kRDd!r}-lC+=|3wyrI< z>BHtyy<$WzZQ8msyNc|T-gek@+9AdWtGQlzq~LrlziqqFfc{}4gLjz$ta_||+5zNZ zZDSMwON7sZGiQ+@=S3!5P?F4!&OI1yR9eFyzrNBx!q+raFvrk%X4W61Cm??#~B#EW{W=gf>R`4Epy0AUPQ4l`F&}JU9!d- z-Xp`(@hZ0znR0kwxx_h#WBPVyr-q0(GS;|`O8QBq^xx^U9VCZ;O#sAa;L;j3~KfQh;)>v!=o?P z@;VDQO5+T{jl4(_J?Q}DALpWU`ld>#hO4o?HeVcLVy(WW8Gw_+f7)K0sn;$|0SS!^ zWQIm;pc(z%YKysqFZtaB6aZpG3d1DRp)AvKWmM%^pu#oNl5*l3#!5=~q|xWuu)wAg8b}+p`6)-- z>kLQyh+OjqNV&@r;DUhiqXa@MLd`OkP~8BQ zL1xBa|gi$8R`6v z`oudQXqiLUgAXXfcX=y&gEUD&W2IsZkhHvWH(*gs^9c(}dig00*tTT@4 zB6pKg$1OxTl?iMvBcBA%PPuIum82;2hJ=^Mu{0dP2XUv|M>!*YZUtLxbbr=77{x>m zX$$ds2rA5S%gja+lmVw=sQkvPuiw3%@bU~F(JWzROXiw# zBO4cT#Ni-N5rF{RhgPDTEuKQ;ZDAsr{*%89`b3jt8^T+H1ixG9YdTqjQ7VP}r+lA$ z#E!9g<)a7n2R)y1l5dSF88!fadFyWgAdjAjCG;>)R2vMZgH;mQ5^tr_dDKM$FcrIT zugLT{hS|mx zsRZ{Lg3$ANg%vB`d4-k;ihwW3&s3TAw76F035cbtf;sU9Te`znxZL{r5ij z?pI8w0jrD>Ile5dUc4HJCcb{E0E_FMw~uxnw8Wj<0O3&%FPH zj)atJtnjVNLPONAl<$%Ai3eyY($J57lRDEjKy(g^E5)`0ct^*P2soq5vqPP%zBw|sM8Bm|^Ib`7(D$it zp5)Yf%c}NmpI*!uv&g)d)ms1Alwj<7g$il)InEa zrC+*B&8!xR{TNpUOM()aO8vk~(jqWyKdA(zSjLt=bRwQ<2{ORA(U{_kY@x~cy~aL#EYn%Q(ysoJvZ{o+RK_F< z*;iu~>->&PG13#}gEX1)pPNNgd~v7 zS@0pNdEUKL(~|%IAOJ~3K~yD$70$|9 zp&e1=vVxwR3t6E~xEvx^NC|&GE$lbTjTDoivW@yXdr!Xp5+~rjWaLoxb8a13TbZrJ z(6;b0zH$SWwQUq&f({L?BwM6~uEm9z`yfsc0qAroE)+dUB(pT4SEp)6hz=cvz&muE zGT=(&R#=^$a3R5?%mJ-H84=RxOGEvLw{^1w|8}ALLayZ`vRBIut!4-y1UB4udM-R= zk!Y?eq2x*=&kq1ul2-GmV-qLuku~ffC|=5ew^Lf{(q{n5CAi}dT5jqJ&pmFt70fI| zyduTxh-G1U8YJf7Qh>0KY6tN?+H#5sKX&5Y$dP{$R z#tu12luiqzZONyQ-*qD5Oj_)gCo?pIReWsrzIdri%OA|G*eYG(8nGU#_u(NL!6Po1 z{DpYMl*RjD$REON`ii%L?p1BHvX#-Z=ihwuO@3i_>k7UAG3%zcJf_wlOUg2VCB5Tg3-_P}IOEZ*!)Xe|@q0u%0hd@6 zq(a4Or7d9m8E>R{O6Skct#m}1kV-4u2@*kJ7A0x3FOcS?%r$2*7(u{xlE09X7%#eT zw-)->ys$&jsfX;%^?$7LiW{~Uc{GL^+iW_5;J&yaO9=|wCI+E)ti$L?JFUyn-{c`P z^iq1Sz>k1#ls&V6=*b+VqlAU7f`XS2Vw*=o`099QEGdobJQXNvTBvmPTUL_cOVVLu z;bWHsbPwl0J8jl0x4jy{pR*1;;uoPnQRQmIl#iVCWfF z%tO@1%B#il-7@MyTqUr5iCYk$8p#S#L^3)Ir07sI9xB1zRO(c4d2T%=PRYaegcmfz zHdi_UM37?%u;8v+9qmI;@DeG?AySE)`L#{T@2wvhK&1YlMB*F(yGB(Hm24S!<>J-rdQFp@u>N zoOXt&ptXh1VMy|e({kAdzU6DLp_8u_u>A*6=>}JXrAIU)jDv@rs#$zj<|UI1VC~H6 z{2EN9MS;s_^7eWpw_ZbM^k%&ddK0n3*H$cCU^KdNXHATMFiX9PHuQ94EB{_aTX7%} zRIdh$CC)YRKqoVCzA|(4DnLZ^b_*pnr#u;#ux;C1GM1@j!A&~{zxs=iBcPPZ?xbm~ z^H9t4mp9vp7=lDLZ2F)Qe$558-qx#U1kwoM2A;<_(>pBF^_OpW?L!$7RIrRiG{9IG zbY8K(f632&`&w&_NfK9uq>93Zwp4Bjo=Ql)JdCNNhOLmPI+DgJw6s^rdgd|ZOxavt z-_j2xms(2mRXIy*`A?HZ8sp_ru8R}B0aD84vABVWJ`pd8&*-)Hiv1a>$m10xCYs;{ zn^WHblH4?M$+Z^7 z+ZMO=H8kNHyb<9rg`)!G&btL7Vwf)9I>D4*!6dsj5_!QnUSOge(_N^eyfTZ3o{>%) zrFiQ#@=0!BMnaldNCra7nLqqYMMjGP%jhjcf)iqgEhN$~Bf5-a9*JEj~QBYw;u8TB330 zw{-a)hEWf3b#v(9B2aY;Kv+UEyHui7jLD3%5MqpdT;WisP0zfo@s>RKD~sWZt85D| z)$*&%sQ?3QH`y#5dkSY;mRW=Fwx-~=-p4+-JHM>M9ePhULYg_Hc=n=llubPw1t*~23% zmo##+Z_0-Be4U`g3N5q5E6*q%cw=TQzN@w>&1rW=eY3AsFl{)+Kl1~Iu1kZ1X0V&u zMoif;>@qg-EU^4i255TfZzrXOY@k_Tac}d5TFEyvI^=dX~8y^5Rq8H8!O(pC=?zmZ*_fr2KYUtfOPL5i>%^bXVR0oMxsUpoGsTBkH&~|%9)Q}gQ&x#O8%jIO z>_J#w1mbLjU^`T=NOGuS+h0I|*Sr+Wm-0^lp)K09^|4Irr{ImZ5?hrMo+HOWqENx4 zh6uz2MFt|NkGKXI-3o_9-wG{us12Lq4~NP-G~G`~J)jgURJ<;D9~L_FXZ=C$Qu)F< z1j-|wdb7ggA}i@iH3-Qk9)ZCltuD_w%;IrXG2$<|EjuVj-oYS|3OpR3u>^#dR<3D9 z0RB^ZHw|B~*6)*0*QNj=0Wrm;a2iahRQ`mUZE0uI~ zunP?`U!gWvVN0bFou$n)NC)UBdY1%CRdC8sH(@!NTqPfJSyp}Ttz(O%o)l0gWXQlo zKcOkzP(H3^j~J0D`O@ay19bqUoB|`mrEpc7l0#_;mQYYriNaCcg`q8A`ru4NVf^yl z`~K4EHk{SF9a_-N=#=alwoHs;h==YVxr9hwlvMrPWz-6eq~<)j=vhxsIdAV}t$$JG zWQB`G_1az!wGl~!aBVTorv|y#rQx+WCyi^KbQ_ifDSB6aaRxoaprk!Srgf&w@SVKH zm1F20@ajVv!f&;(fRu3tR zOeu`cvc|Pm{Nd!UHX=$+xP>u{bixLG$dbkufn?+?14~Fj{VwFBwQx(Uqi^o*@0OLaVP)I&6-M230BNwro8+}5MN6|d zQJrKqgfw|x`E-s#T98xeUT&Xd}xUie5Q{mBGdhO?a^cnCP=qJh1~`=OW_T-U?X&zbt|xh_nK1YSHOJP zE(w%7y{tMMZ%w9;(zYfj4$Y?ifWpt5;StIHBmCsChPAak>H^K?^(t;@chtA@m$uLi z$^m?d9XSG4h{Y~aRI{MLDmD0$x%CJ$@b>W1rVq~O!#g7FKGPWAoNQ&s1rRXMwrbnE%I!(0`F z7m4UaJF1!NXh(#Ok&TUQq@lAHanDdewl3#+2A@Dwo_S}Vg{tP!gvN1Fs?1TsS+5kR zk=G3DVBu8uOEXt6hJLQXj{NFG!WyvoIRq759b%=x@)=U0kx|>S<(lurrL}dCR{{|? zc0pK4#qG$?j(T)rU+S&m5S?Y5(|!%QZhN`OSNS(xF28x=4!$FUZICik-(3b+QYw{8 z4CAAlbanZVWAYe-R`A1f{mQuRhcQ0 zViCeNoP8Ve&n3X1Std-qjA8;?>XT`nDiE9^Mu{9{LdIW}Vbq_u@D7X1#gf+$N(Z31 zRkTWr%Lx-jJUi{#ArON93Zy)xko<)YbvI?~XsgIxTU?Ztq!gMKUNSl&`IF}BpzkBC zbT#=33;)Q_7hNo^-~+#fCoRfk#AjnvKH*BC@G)7X0|?W(lxnkz9Qu?>oq|hQmv`>e ziSDNidfoITFY<#}9a0xZYddHPmjqNM40Py3N2&K_lc9iwd=nRSgRV5%4B83?kCk2I zq34fk9J0sXqATbfn`qmKuMCEfpw;Y2q_oXDIFmRXg<8CZ+bMIZ=aw&>ey`KAvR1z( zZI@POH*sqRn-8@ez(QLNmY=D+xP})V}*3;lMHC#YtbGq;AuSbrc-u~Jg%bjJ#$!N=UD^i}V{CaA0 zQ;N(r`es=9R6$+V#&e$EM@@{S&PnbuZ&6z|qhIVDG9LibZQx7p0=5<0O zKnR^i-jsXk{3(CIm0;u=SY?m)%PLGuIC(}@mW+?KBXz+GHrb(a;k7uallhd{C4%l; z`0klFD`?T_JhB94W35i)BM-IO&=TGRRR`feIbKHU3#3s?HLsLxU)vG_x5E&)R@FKv zPk7xDV<-;t*3V?g8d(c2jrW%Z@5?VQe}r^(fCGR@{t+OJQl_iK@y+7fR{0z?8bJ!T z9Kk6y>mHBs-l}}brjo1JmS8kp#4u4DWFf9nQoSLqoQA+srg(LSWK5Yuk=2p~5B>Laele33LXqu<#Xa z>fR!?Cw0*vHtnbb3=EFQ=>~X0uT9t>$7n^Yr^Bce)_PXR1Vt4@3DAs9`0VKu0h=|% zdlBgLMp3rH(koYhBtlD=8H<;gc;qAWSzzPhN_FU-$>2sjJu;tS1-0~mtie=SRa|LZ zShYyXUug(lgC7(wVpRSsOgRik-2miP8g*q=#-r9Pz7F@)Au;=tP+uM|}Dp-L{hUVUT8Q*ot1#2aQyqdg;!*)$2?!SvUusK;)5idC2JBwh;~awv=W6>)@+q1EkZ>hM1@jF{O_HahZsxi_xJz(zIQ%v z-h21l<(zZRIrrRi-^{De6nkq%=E}+-eAiS;SmUu9wfX`k~uYRlQ6x&aZc* z4WG7ssuaQ{|5n_bJLA)Nxo44e@2qoQE^roN=(R9Zy|}%><-ym$3BKY}rGufeh zS9U3ZebFUi^WI-wSL)-^9jcmSLngw+eN=XBMx*iCO{8y*Rx46>942=VBwdu`%0`C@ zWq%QLT&$~MzjWtYP9x3+bc)B5)7Ec@BRdvE)X!UAu})c;l|zqu&6k{5Lt#|g1 zU@1JNkj13D?bcv=R<7fo?16RVDgBcy)Gs;!@8e(XWl4*77|XpcWA$N@WP=;Hpv8$?aKn zy*t-jL7BAmQ0xF|?Q=oFtj0ovQz}MltZxk*%)K03)|AW@dMdqfaI009>5eY52vg5Z zo2}YR9`1D;T<6nWqu4<0xI@L(mqzJbNtn&uX5OIy?c?($_DH4$9ny5p*m9%vwjE|^ zL3i-!=)GD)@gW%0VSh>$26jtIlUr}tKh)MBJluI$qg zs+8%V6uPrv^j$TN>u&Koj~>38IveWS{p&(UW} z?;A?3*SCj@XPJV!4?lNHaLw+E8CNU^gs!QFxJRcAs~TPCTXylCm1Nfl=XG)JJKQ{Z z4QdA%E@iwE>x~h6k}H~{*Lzey;QUq>RjSkjVP6j~@VS1&@_I|SO0$1dMKqOrRb}*j zv-G#NjHJec<|Rj*!j8F@epWYKX6@HQef*+#Xd;QsJxkl;?N4M=9*wenImwC&X7!9Q z=b{@hr{Jtfmaz`1p>btcEF0J(eLed@|EI)^u7k^Og>J}dS^hpV<%;RXs2BRjf;f-g z^%CB(s?J?UxJHrm^z-@v=IvTLMC_mUvDD1xx~BHQV^E**>_c&d!L{C%>>AJS>iaz0 zw=%B6z?a2OKhA`F450J=q0jBwgvl zmMg;6)1%5nd(v0ju)Vjn<+7^qy3?^Q>bAd6^$ z==_cFJjtleLe*rN!BX^d{aQ7}c?vJ{NAJhpimbclp?An(^&_rlJ2I|-Kbx%rL6GI7 zMXgI8c&ZIx8WgQRXJ;pqTkF;C&-X*6dtM+H5~>@oCm@qrBSaxZadzag>^>+xe8o%kP(wuOVJ_gIMFKlApL5yS{1K zlY6giaec^_>IU`s`|wdAzi)3cAC~PwvEMTB)mM1~1TVryQ*yan>PHdGvXad2Jj{|(y0b{mEId>i{fbJX@+DhNNYSZt z&orXq*(#5EcWpSyD!rjFR`AZ3t95UgubRh~iLQBB|1#W-+xw9C{uFDfdaCk7ZslI? z5IGgIMNzvy#=PRJ?p05{a>Q_R$Qka~%jvu#MSTluJ^9 zvx8&m58}%q}W@=-A48UX&O2 zT{aK6U1_yBus$cQk4RVhJge18y4qbGt4=nZuDB^D3Zd55lA-hSerL1xr$q}ZesetVyT7w@~Pz1c1{ z4Fmoyx6fCkJ1z2T53$d&*I~<9mDt(9+vioFnz=}!FF-9S&MQJuohqPU>nU-=Vl&6$ zZ$`>xu77GeBBox!!?raw2azJ zoN7;f2FVp`P)rzJ(HHG?o+G_c{_$&>cX{rL&$P1@`xL;%@^a&n$bCzQ?c7?Fieyxo zU%T3fMy)c=+IQn?ap0=s4~)osOLOD%>YwSHSWhV%FOu---1=~|P`zrNNmh(BrBi*% zHIoCD!%qzKt7YRpCQ|q`m&gg!AKQ1Ktoh^~diC9o$u`?R@t7}j`C8ZY@JiU311HFp z*nI81ZLaarf4#~~KiYC!WcMNM>mQGNy7TIG>`2Geo0${?v5cR(oj0%>UXO0OL&+m) zRI^KTgUG;sW-%L@PW6{2gXP&7zDZicF3*-FN*0{7lK5mE)gWdYp!d0@>WL|nm#dSG zaBe{4OZF|M%h%~sN$8wd*+}b5zpp<;GNt!%?mFLZ#}}Bi1-f7L{i?f3_2fk-&G()? zjU!H_cG(Q3cxXZkVY#?z#H)=$eUmF}+P8^Lr@Y+Plie`2%Jxk~}p9ZxU> zon%Z~|4jC>k%QLOH?lk5Cwiw5*#y6_h;HUizi$}(@+)b4`e~vUj9<75K177as8}U> z=9sVMBHf<#XmO|8X4BEMFKyA*wU+IIDZ;gW^zIsG<_r1^ds_tW9w=MdL?zb};!ttm zpweMt#y0_+mv`@`k^Y$SKycnO&u2P`!9~~WJmTybGs7O~8<9}$m3s6kI=VeHy+2r; z(brGS@Zt^T=m_nS=v~SD4ATQv ziynT;w5c0xIVp#*1(Ll=;MTfKn>cTPmg+*n@qo9V!C#ux9f`_RX&5myln~0UXsz}c-xur!yC3Mfd^~ef+(>SWoIst3@1uL-x<*Hu{NmNnCz3nl zF2-~9)BD)&-}H59P^)gOw%I)7|&}QS1G0*zYe{SI`jBy3W`>$jYlU zYOAP`#emt#N68e6&5TtKx?9?=cK3Q5o6UXw>GJKKm!c{)($5bD(x#Wv3-JnCh$o-A z6DTf0%+wpojXA>_MW6brm8!YO`?gWy8&-)z+ltGAd?yeKF6^0^^vuCMx08Y%|bRsx2i9w+a0L(DgU@{+`69K=Qm$y{jBAuk0Cp{Ki0p8 z>SnolY+#k@aT_yj-e$ItpuU42JSC2q`^T{~DL02R-$|>H&7^$Na+|1zZOh5D#o+e^ zCZBXA!bk;AKW_3kfidcGyWO_8HFqeKw=Sp3U?Ju6sOJ5r7RZaPjIXewRoy68_vL-^ zwk1!RV*H3Vq|+=Jx}Re_@K`&oI3vlEXnr&4C;eWvQf|_gdRG*~aD{d9^ZUe7RG!Ul zfi{wxzjn53Rm8kK+J^a3+B%$;w0qF7L}p=M_ql7VHLNK-tcp$bbZ4YlE-XC55&gP2 zet$>%)z9%Uk5%s7_pwD6y)Pp^VXkXmdU7BWL$+`I;o&{ElCOw0yYS_7yh&EwgR=W< z|6uv6gt{!cQ*P^aH-VoaJ|roz}V>jC=O=%ja#0*-k&KuZna_{YO zrtmAiC#b)WA9$_HZ0Qs6MZk667n|r|X1gZ+nj0%$(>^^pz~cRBb4B3574UGj-*zsi zlJGrc_7MfVjNJi^H^udY@Af^i8`yefuzAB8!E5>95Blp@cc14i1KXay(z&J=E)4Bo z+I}>0BQ?>$TOOV_RqG10fEdZ__S_cOPPWV;foSIz=|MBO{TCve+>UL3xJ|L0r0-?d zmC=JCtRphZpYJwQ{2(CtHsqD_x}(pxRP)=M7qp6{))nP_!CzpU_xR$=7`g7#WU59B z9POf7E89wrUoi3@>2GRc(QDIk-pM2*@HD|Rrp(QC&oRl^Ah|tXUIsDex3t)9ql@OK zytwP3{>>MZ+1p1~gb0^D(d}t{pt7m|{psK%HU4xz(b}}5pX*)sHZw90>OCznjAW*hC21bc zrHX&+7?SO`*rKRdwW)e@ie@jRuVl5MRLxY;?*+;H?nwPnnee9V7Bq;Y+ePz6FIXDS34*iLdlt1eqykj1s$VReBZXOWE~X%|ta#nM>;HjGh_Gk-pG5 zXwUiTObEI>zk)fg$f`z`v|o48{M|H9L`}Xu66RTW#Zl{mXVFs=--tL_lb*Zz9~T^< zCVm++817eow5~c|b*rj|`2$8{&+vwKt^VeeMw^RTWwNRlhpT?o@EFdCXh^1eAX~p) zxZ8(YESZxDHPqO$vqe_dtV;ylKfjpG*~>fe+sCx-TBCZ!awFDahP-5H(p^kNjBSFe z8efG~cayXPta9!M?Bgss@jN+Qht0d3I<+ORsbt^F))xo;hloVTJ-I$q z__|c&(6WC;6$)RpEa^6}UL>g4No&#iB*63Zs?JsC9Q>UWzSImS>33+`Ro=ymIC@IuCCnHM}BA5GIj-Ki~HEPa5sDuu;) z<%gF12W9SYkB9bv-`gCmD!%r5#k@w^=yKh6Y;=din%&e?9urj-1=*IlUw`Q(rqPem zRol-Xn9?|eEUngqJ{QG`aIVu9OR2*GZpm?ytphc2eYEiP%V@-Z=JFv zUE=}&3U0m@9*fvag%Or{_d++i`t~BZ9YHEzx)zyk57fa8HKLT0bfI+cNt;5 z>|}D4SzbX-rCS&;hg?~hb$ikLGb1D>B|Px ztBPf$+na7-J42c?Y@euj1ucINnO=i%EQH_&xRjd&X?D50}EF3wK+)!K$7c zre2g+7HRD8vF4|mH)^X8rTyS}jwD_ELrodB^BF;!A}jSQETzLTdrE8z-H*|Q_iA!g zW=5gSh1}P+xHs4ri?wasz?i%~^@*-glDBxvrVl4pbA4H)7{^&nt9aETiTm9BZ%kh) z7}G!LCNv1TCB~_~X(?m$;p;QEW>X`*RK7=M@%63^ZsFT7!>={smT|1EmZ6$|#AMH% zwI?5O)F0V5u)=lk>$C!ob!I}hADz(7kz2>qk#V2YYwHQ(t3Dlu4KJLyge^i($sZT} z_OOcf!wTxiol=fp&g$)ylJF zpY4l@=!Kr3C2?Gd=r3m;B;DOV8ryKod*^eC^(xn8PF^QDS@`k1u+NfMVG{b>RTt<2 zLwm$KOG^wsims^h|HAqqb6aBfEsjOrodWcVO9xC$%)fLl;*MQ`-b2Y&@Zjv~Hg_cs zwUJ^s+sn7&ANf9PuRDBEwE1&qS*T`oYKVIuacXjiMIT(%LDUvki7PGX0?@TO84#SBfa|3ZTXG)Y)7^pvG=Ob-A-;` z;+s8Wd3IplYqHI|a=CKKJ}vdlcaD!g+r}yHbJG58>p(yKP+r&*v22M~(t%sLSE{vD zeh}MMx?X}N(j`cJ_nJj=jhc$240POj(*2CBF%dcKtfB4(>%&hQEo-E*N|us)+eyLW zd;2^YQRXWH^N7_isUvt{C5~H{AkUWc&QMP)dsCi_FZB&aqyhNdl z-}d5hX+?ok=PIkcUUZHc-#SNTX)XUMd4RgU%=+8yAlcEd2vr^KmXFQm$6k^J+%^qU z?%=pCzbQB;eBNi$rOeNo%e|Ep7qj*!(+662^%vE&TErQg_Wy8CYhIL00Yf>bcwFd> z8x$rtFwaz9W;}SBR7w*S#9ZLQ9lxzw`GNJuTieRlzV6y+&)HCs@2%z?&zS7AgUrQ9 zz`%Q7%Vi3^B%K5({P7JqZ#Io0lugYcB8$el?FDd`i#krb2Z1h0n^iPeL@F z$LEc_2s3BbQod(uB787OX(JWwc6Fi@^Y!znZx64pkyd%YU*Tz?>KeR)g{C4YnaN4~ z%HFfIJ)hS6UBrY|W7EelRpzz;3!N zd|oVD&1!R2KiS5XimwzRqGBffd(cl6_(z^OF|FuTyYiJywm>4P!=gB#qPiw(U%IIN zk_fh2Yx~|=-iv)(#HO_QLT7E+r-!wbt4ObBe|Xx?MjX+td5-l+HRl5#TF zHF}HUX%@Zbj@>U`Es%P;U^9eQ!Gs7SVAX>dzFZ=2wnb;adEndfxr6TseobhMO^+w+{FW zlONMr(|Cnj}Uc z>48Ox2aVp^YNGRS^-ZJM%qoXt zJye7e$!QA0QnN*5*Y+x5`i@g}9$r!4-b}y6n(agd)v)A7yAD04y6#@HKBolF)OmLo ze}3GP;os@-RjB$w@5t@LL1h*yFJp>wSGTpKr+DNP8F=&6`^YF{dMoZpw%JjpaBKU9 z5*J39C%zco{O1i4rL3mLjSmDUn75IOK4`AfIbqkLJ_?nM35!{n8@EL%W{( zlL+2o`H(K0y;Y=?`aXqt=(W>p4)m>hYEU@TLrr|ha(zpLn7b$ePPfOV(i5aG}bBYUbKQcxbAp)uOy~C;a?fKr^VvleFZ}= zpS_!#Qtil#LswJu-j%t}o;1IRhT#^*k$L+@cb;!|!QW2W>uGtmdcPs*&y;3gCqZXj zQ0$Lc#kJPenu9DQUFewG``eF;+;>~=VXS;8dLuF5lkwwi9I7Notw&eOv9uH?E0{1S zv1w5IX%l@s5LU;|l)@QP^SQjY&+3~i%R43IT_dt8m)+>Y7bj7#Nce1i=fh-yh?X|y}Dwx%|L0?f$Qhq(cgD>NH-i{dnGV% zCGXiD>df0BjrnbSnaZBePd%qR6{Sq~(b}HoQ1#Pc^Nn9COa!|M{pAd9FbLF~j)=*O zS;`|qVbRjHi(Z3ERoNn*g)ci!oA1z`B=?w&S2iq?^*Ffty}rhVhgtJ{hTdzQA6{}I zCI6~Ab9eRG&uQX1mb5|3pEN8`2xojipBq6JMZCT2-ID9($5J$Hf^}D{e-oAd($Gz8 z1dZX%u9?SSvl^vVmG+G6^l-JpNC-`Px~b{f0HrON=RY&`>ae=j*M_Xp+O<=`WAF9V zynGD)9SSsNRQ_IBqpT)*_RSCLMYOc{ycq4x)aml&OMibSbm#6*o>Y{MPE@O@cKOjTdv18&_l9(3Rne};ZxuLf_>1`yuW^2oH%ROId?3d) zlPZ$M;&P>TU+r*_rc7nV^FwL#4JEEsG3>F+EA<(^ajo&oNiP-s%m9|pudUDa8n`e& zG7KHK`s|7-k8eSwhL)q*($^8uo!5Pe9S+!Zet6CN<=}_DeH5FkY2+P`3iHwd7w(Af zDY`=|Ql(@BvWS)wCo-P*qdmpaD8ZL{KS{QQ=XqnDZa{am*9!re>U*WbS;QyKcCj38 zPkEsDWK_dYJ=KZBg>T2D`|hc(+T^|EhqU%>C9A%ZP-pI^S*7*dBGvO^QgKK3{nFFcMKzIP`2QE~C6_Q$*jceR%>SG^VuE@+hBP0~!NDP0{*uE{zW zo!E=o^uEHP{exmwkML_1x|_W_bE?qgyY%b^{OuHciX+Gy*KIx@&vRs#&w>0tw6fO7 z73JM3wdeiIUs&DeiBE|NRxr8L+~94YEx$KLKs@-u^O2V-+3^oI<$P?rc(tv@oVa~e zfkT|t?Gdh~cGs*tnc)KqW4BRWenllLTdtXi+EGsn{@2~8&U?#|$efh5dEg~_v~P~b z#|KI-Zr@ZWm@S#C{r+h9E;K2sIE$U++p7>0zd^`Iet!eZqBZS@A*{ zm4`!i&kYaV=2#RFc~Mi-C`q`u>U~E6r-FPb?P8;OF;ZTFnwBh_`wXJ@EbG&(t9~Au zwQtxp<^WCFU9$?An#Bos4Ed<%pQ=$zph_co7w$_Js=n&P=jhXsX9sFIO{80C%cvJU z7SVKlp772_`F*vU`x$Frva&B4@)d?AHLWZBxap0Cv*;r#HC>BJtWWlHU2EWf)9|pj zjj8V2=Dv8!<9p*+i;uS?AH9}P`Gz&5$yDcgdPV1=sH;KLT^*0Smw$bBqM)#tjJ8oO zHM1_AX1U2|_`w%Lxj9lDL@@($4(I(p>+HRgWY1eIjCr@D%4WaxvC#eHpEu1Hw2mm> zrx0O0Y}aVKGmUCqnB%Z)sYrO(NTXBXw;l(Pcku>W$y4cV7%e)j`G;T9`98Qx_0(wS z_NmB#1F@kl-x?KDglqJA-eud`vssrI_6@Hc-P`0un-d$nt6{a2|I_4XnUVf)3;cF$ z2;vG)Y(C^~sYb!qlTNHn&r9Vw98&ak;RDt6;TMxaZZd8cqPiNef5Clvk-g$Cs}?-k z+rU9w+g?BT<-?6|$J9Gzd5K{{IdGV<+EhD!=dA6@2(= z>W5*|?k(r^NlS;Fs(7+Qw4V c>Uf7fK$l{-&OqLe;q^UVwx`c^gUX{_4GLy8iQ$ zh(4GG_;!c(wpMnuXfk3R6xK2y^<;XJ>mfauc#%i>TQm8mMs1mUGWpl`zwEPn;CZly zs%XH(Z{UN8-$R4Z(F~7~B{GHCkM>`x?nHs4rlh5qE^oRQ4Xzi0L*Cv4JXkdHzw0f!345#wqGV7Q()f~+yBzK5~nk>DAFyYYNS@`ms3OcdBrw4gOD4~NYd zXM0-<*ols-lQYKE-4-A~1Yo=uWGIO`}#7rnZH%_ep zRmYH41P_iumfAWwf@~CO5je24E8Ck{yP?RyEg!$Apb)PJKNhCurfBAFrtN6I8$|+O z%4YU%7!)bIvXzGr)QjrTQDQLE@FR#?i9LJc?{T?k&?<68i@#Dq*`b_J7}Q4at$=bs zd7`XPE+{u_t_MmBWe)PqQ8pmg4b&jV*V+W`oIyF5>jBDIq1rH8Ua*@6}vK}%>9J-&o0 z;Ga3*qb*7aTh;}PfH}$=g|u#pQbu`!T26q=<7LOkfiQIb6pVguv=`&sQ34pa1N@;> zxM0WI8DNA)%?3w!H9iW9@GVZ2G&G7557P#cHBRdmaOVuT0;S9alpw{I;siW`SXiOf zft&^S-0_}%j$))8*fs`B^?2J8^AIc$Z6cbWSqMx6C4j{dwnJ#dOvgqU4eTSXrSb7( zM){yrfHpb-zC+p}t%y|kfzpUJA+k1ISu_ff2H3+2d_Vdq9iTls0cNmu!mXww4vZ2}-5{IBdYR6KWG)cG19G;iw@5Lq-TWkN5FomI&!X5!^g)gn%s(2lpai%!I)VAq?*LKnVr#Ee^iL0bBrli-LOa7}xJ#r4zw< zq757$t(l?7XiQJFACMz&oS%K?uj?GjC z;Xi`cxF#W2$UHHTw(-8u!1?+;rl*1jUxE`@Kxr@< zJV0;8Kioj}2<0je%#|mQ3+FM!KL6_2BK0Btj#v)_dJyCO-{gE`{z0fmp%KYMTKZW# zjrahYuyX{QQ>Nx1vkcNZA~)k_1fl~hqzBMA_{|J#nH5%AZGg|@0j|+_uXmizpT!!n zImu5cbQ8mw_}*iBzz6~4r;auCrT90wKgwAx^$8v6IM!RhdLAUquB zDsqL)HqcXn+CR?E)xU`bga;eJJS`6_8}xc1?$hNX{xJgo*>Oa81N()3i#0ghzyZA~ zC@-_Y8D`}jBF$F7HbZX;jbfa&4$>;l%6bBMW(M99Zbiim@N@>U1|=Hj^)Z80m^0Sb zf-4Z59y86_2VzOS_hO**IDpS&ZxIbJz)8dOQiAvug1|=*0`sXT@F0Z1oFoK%2>2}s z&Z1z(Qb8$#Z+>tHg0aT=6^fvaI4GqAplB4*(yxv#V(+0Z0l5YJ(Q&^58QY0a<_U0? zSt&*|XMCKHQJOqPQ%7j+tgR#Kge?GjH|z=fe*K?lW!J|=Z_n{Y?g1e2v3&N?%}IH&ZFSoi5y@W?ogdv1hR zQKo>0aP8&}B-9P?bbK`jJyc{xjYc8<$J|OoUjcrm*H%RTr)!TH8<80ApKI0@5g*}u zi!zzl%2*5KhPCeNXKfR)1S(i-A_6RiIIs}>z?uky98qvB4y+14up{8T=fLI&fHS=2 z19n9e*b}(2Q3j>aDDGMN!bQZ;6V?b5{ZSsQH!(ifkNevjW^Eav0^%wUa1#S`K+jMJ zU=CMX&?1V11Ja=k&d^pud#C{Vg!WPilobJIH1H{L<2tUzh?SVg^>JSo&S@S%s@wqA zk+tIZtTqt}EgvqXfd=8u*`x>Y{wDTn3NZXW9s%2T2YJvpLIWCRYjvDY0@&53Jm3Y6 zPs)HZaCU&}Q$g^7a5z^eVmStLE&w64xGz#GwZoXhfeP4r_U>&$Fg`ep)8Q~gSxr+k&p-zYZ z856>C3rda{Xc_8;FyN9HkSHbWtjhcj~1cv6~VVSuARur<;P-I$Q(N{1~<;#as8bR4mDtA{vs>t*`*r|I6GTwh+Jy} zecuH<#qT*qSgyrqZ54@*zK+Oz3@%27(#$DDaPe<0}f1$ z)Ig6)c2+D9i9<%08MKQ=$<11C+|taAAM&%dh-mm^8G&>1XkXVhTAcR(I4Du83WqT!MB4O z|Ns1X3t1mej%+}`anr0kLFOCid-8(=Mugyc2gZj)!5#G4+BR6>KhCcL{=PxYoz5Y=r=y zV2lH8%6Ab;qprXQI?(*4hZAP#>W+ zg>iYPvyf7_n-617aEBkRuc0Ig0ttk%CtS2%98k*-uHnpVitp?Hi-b&;a%hXq=FBfh zwFp)wV4My<9f4c_Kpw$$5{x;)7zZnCiwIUT(3gE0Y1?JB_8T8^e>?1 z3V<_=7s6Q#4SciN`kG#UA+FZ5)}Ky)VQsh`n)vxoAGOJn2WhayQ!x8z;o|quz9@qa zj4$AJ{9s%GMjc>$PZ*q`CMkgNf%XYnCb&lk?GqZT-DVrDsX7I5+&ODKLX_FhS__dw zC}mLEpic|udR*)k7dw#$(gtS{D5Ef*4}D)a$HOvEVxerJ!P<199Q^-X4?vxAoHO5W z`yFu3g}Wfo%EEDnJ3Jy-jfJx^j5on@xQI678l1sk{2#6Woo4N8di{mCI?q~vI{k&U zUGTMkHFARBI5n#54tkgw>N+b0%kd=?0JR?A19v7Af#t)kYu&&#?&)Rt)CC$uIdGEr zt280&VEAl>9_RxeQ};yXRtj*hDjZ-n#p0=YV_a=bl0K^R84xGafF5*TSjwj^F-x>k9zlq}}Ty-Pd zBs?kzuF%IvMqoT|d{lm%SATC*zV8XnoCwV`(FAu-apeTCG7hB=`o~D!ACU>1CXeg2 zFJ89=u$lsOeWHX4j>!yyAA&I<{f7JaIGhj(6PzGTh=jpCC|oJ%CE`YskdDK(8dC1} z>o|hx@0TX1c~iMOdE^n=2=Q^MrV{2OG8X^6eEgnk@TrH%@-=Sr$K?yrjQ<_JAT|%t zJ3^LSXiS%9<_nCD%th~DJPKEi(A+w1+2L9VDfh2s$9V?GY&CA#VQdO#3*kBx;n!R( z`}cYR*I7slNUMZA7{a|H8UnyJ19d?Epd0V0tw5{Jtf_x6P6D3Ne~38_2N zOaLQD2;GP^m`FDv>4Q&2Aa#C}S8$&MsXKRGK?#7*SVP{zxfVuG#;_9fLy7PHqc&f3PMF2E9?pLtL&1mhXgXAh0#qwfuMu9PcJ$0Q-Q?Yfjfc z0^=R)8OLM7?ciYJWeM>Jp#TRPFFTQk$TdfZM@>c`ik|6N`S{O!~C5=qEwfxl3gi$wlO zuLVBUfsFN!+UWmouLVAr^sm=~%uzVYO4!fA!DjYa#K!y)#6}a+2>2w}Tr}cOdM$8Y z_-8fZzuRkpJF@?3#P8CGxp^({w_1-rO7KxxWkT{#`a&Y-}YolSaTeB#aV5{hL`Q{-oamu|T}1nfIay zMQi`NJr~Fu=+z-|@o&WC*CH;EpGbUeHqDru|MDki3y3W;v;DYc{CCe6kT-BX|5r0` zI}kW&{`qz8+`M|ob;N_2dB*tHum9Jt|5d*pN*k1G*v@R$8j1YQER}!OkAe99tatz4 z?#JMsYy8(*|E#s%j@1I(o<4jU6?wu6@)hS5e*fGW%tt&Oc#VsZBfIK2&BW~>!zbyG zmGSs)2COsgd(3rh{{3zWYy)ZepTuE&#`S60Obzx zW`0HnVV&P$k;gryAD6@5g9Gkq$-gNJaK1!=U*RCHxKMC%aCUM8@9aR{ld&9t#$I7D z0ZI(=`26+-1E_Y3iwOS$=T zEC=7pF_-ohOjE9F620)<9)#MPTn@e|WG?04>t5zk4!($lP`SzD0bdp~mvZpMGIJ>h ziebwIzzdKN89~l455B|(Ciun}PH-@TLkoMigDwYL!$fUN!Uj$|j7cEH^A_pCOe9Tg zj1pwF0;WS4d_fI-vkToecn1d>r-Gp{V!?1Ur-C7} z0E|T!MHpT9MjSY7C?+QOJvJPV0nK^X926=Te}>3llM48Tw81n52yI6x7DNCD2V)|) zV7U>{6|96L*bOp1uzVsO0EeO+e+{{eNCG5*615NY3AAI2has9(bvNzXyMKgiDB^PhlbiufiAN!k@=a zH}IE}+c!jt7~wCVQo@vvJAuES#V5$u@7|xp%{RuU1Nbsikb(m>qfC@dXdPZ6oJCr~ zN*X-?SWha5Xwq0gI1514WB`Zwq{0Xhe)xRsyPXvAH{tj*EB@?+Kaa^SY}o^!3Aq4M zDpmtfC`2Mq#2^zXgk-wl3vI?$A@GG*AcTmqE+G-=_zFmC2y5;VXb79N5QQn#r?_33 zfiJKFe;#WUg$j^HBy-xXA+vwDYvavNZ_D0szCA$(Wfk|_*_3TihUa0LJ`;ek_C3t|n^WC$9?f&+{R2_ii9`1~oSbkhJQYyVKW z*qxd8oz8BOLpTNtLRpf)*O-DnP_xk_JP?r=L__Z2^9h_p5yOT$HBIHq1Zqvl5>$XU z6Vd$LC?cK77N5djPNr`L6t9iZ_a{*ll}C{L)4iw`!=C=!sALW_u#9|Xb# z1@biZ>U)&3@Oe{&^cxV;y?Ef{R*}M3%ghQ2F@Xc>9ZZuo3kCkx1D6n4K$@FYvp7i@ zQV+<*s-}PFJj-@MV&6m_{t&{F_@2iQh7&S$D>=Zy3=Zght^>y!a6rV@gM$Se+~C*% z4jyo<1qUxUewd6Jd%utMkNeaQd59`(UzC{k(_|o%$ru@MG}*!N8+op!xn};gMZVgp z0XyP5!Xp(XB5MP>f(aSGeYjij?*Y@KUHE)N{15`hd^#_D0i?xQMPf29`SA@*mNxX5 z!Uf<!J`#m3g!(8mmOkWebo~ay!jY2Vki4`0| z=jGH{oewC5=Y;8Kd%cA3+Qb?Sqzt^DoR~mHw#1pt=cw&!d)s5B`?^Qh5XyttyW3zK z!3j?g`p*!A?Erj1q-R6?bc5mz%lR4FsX>@+H9`pLhu}QP_JYb32TMJ9aUNt&Rmu^Yud`hO{vgfs?gW8DD8yZQYG8 z5R5;MX-YRxhG|tf8($j1KLCF}Ro?&?1E&K9{EWWY3_3q8v3a1D_wF2PmJtgDf&%OW zL7o|0vz;Q!ZctU%&R-!?b&R@dlu+J3p>J@u36#z4RAQpi2w#c1&NHtc196{eR3RMV zf2+2pEcE55TFPFwBZF)sFA3#AvHdIh1`&pd0~}_YCaecr*)+mf`_5u6hGCU`E(Fzz z4O`DQ%2ZFgy6u{)Zo)lC(BwQw&@bpAsEvZM49|qpMEM~EpO`mhG@;v|_kcMJv+{__ zd65Bg<>|hl0Hc0d2Jh%+j!gy_Qw#2?q842OSF7T5A zG(vbw12f%<(X{hA;NPZ$o*#Eb1FT5`BQJySAQvH ztb+ti&ihX^K5(6ap<>)|UnNWKQqR@_E#|h1bEVnG_zGp1ccbhP!~Wc^m?$|&^W?mL z-uOVrKEnuwnJ?S6ao^sQa926?mf0x$PcuGD#J}74$g!(MURlK{dt6v-rNpM)qJ)eO zRPg+@Ddd7x7_~%}5np+W6GVja{t4r=7T}g=!bqffIbS)PBbg@TvXI$qqYB{=|H(#+ zP~KlRK2UikTY8A_Y{my7f0C{AQ-y-6#^vD43+W%(Zy=QSpJ;qwV8kTwsq+iDIE_LE z*4Ks}M51%0S(Q94ie8LM{hY(L5&o+Nf{^CPdH=lefnlG;d_=@({ua4-8a?mT!S=4R zQTU%`e4K#@212Akn&SC5|BUBTT3m1_*|kY+|4ZWmCQG%k-3izO;`u-YKl}cX#QOUc z>Y-m{@~(^8`V-3gCyWma{FOhrwI$^F9_2{Wz?Tij$l4+9v*{`!qeUq1uNxnzJd-W` zTnRe-F8cjedtK$jc~YqOO<7WqpvigviN*&;Vz2tGIIX;Hmwa^Bii_V?t+kpf&5`{~ zq@Bris(FJ_tKCaZ4?&tI=l%1>2gamZjj!K7z3sflekDro#KIL={hdinw?YD$xGQ4pG1g$sjcouMGr&H)VL34y z-}sahQ4-@Eca$Fl^&1zJ)!$KfUnVL}G zNtWw47|IWShO-4WAfFQU*_LM(aAyMTAC&9W+c#Y1gq*Tu`X*v9J@_)(b(4+cPt@PpT{0kJ)1PF1M1CTR0GE6A%pYR8n0dAAS z>5%)gF~1TP0;vFZ0xv_rrD-=B?u90q<9`)icxDiKRv zlek#go=qH9WL72QqY%m?0SM^0Iyv)!GqUvuL9%(=|w~+=3yL2aUlmZbl&c_7JVf>4#`WDo;ln%>RAjdJf;+ zFNmuw;NG<2s`E3maPfZncb6CJQqJt3rxQ%Ct~QZ>h`18kt09!P5D>W8$=%G|*2xh^ zBvv%BPw`IJX@SpfBb=Xnp4k0l*yQ^$k!1#u$$MU~@N~n9Gz4kriVEa9;(SCYtU2G= z{`s2*LV5qYA{&Ee)h~!_Gw}=}%LLm0VGDb!ouy$UCQ>WmW30zpi;W2rWd;#hLh)}x zd5DFD4%FYWuxmh1lXnbY@3V<3#Gl&vKIx__(dt1@!?p%~-DOB9?|)ZZQy?2b`cvZi zQx^7Z*0~eq_eNxIFh$8!&>AEW5?4YSQiSp*TG$yx_P1MDD6*3`_8_BY6In>Z<03)P z7ne6{1d$Hq8Ca|kC6xESE3$_n8$tR7k=2>WDUbzi|FDIXKjlc(Dt2G>7;2xuSCWbx zLLy7(@diS9h=rZOA^7b+EOZDaKM(@4W))Y6KmV2^yUL~eH^y^l7?<|!Eg_Wmzbmd6 zAR9sY1#xu-b95lCm8sz?*`M)YQ;nT8)t6=~TxMeL3>4>fAtbJZcHIc&O|-BASS&%~wvm%h4FFL_|a=?|)Zhk3u$r^ru9Y3AFvg7Pc$# zh(-GSCgtn)JLa#nAsa#Z1#v9`)u*+vKjp*reJx5~NFAlVFTHKUbETtkgv1s4 zqm#E53FS?+urmaYNz2UB7VPeU01tLg4bDy~c<@+x*cd?oY?&Z{ z0uKDOQUHVPs)GY2$R~M>wXLI$z1eP*4>k|?t78RwGdDMk8;^>a z14h{ue0e#!?&OiPaCdUuV1p{gmxPj_yw8CT#+jC>Ga&l#Ix3P6& zGQ-xvR+tVg2>cg@WfAtLG>B^aDZc!JA^;^G8fuuv3<@HS@Zk~#Cr3A6l`NnMV$!v6 zwRLv?3BkNGxg|GjOY4D1VxX8>V3^#UnB1Jryd0Sc`CaGEn|s?qf&fhqzA(_y&#B z_uPj$q2Pdt865obB67m~NLC7l3;qmeFqrglC-7J>{vry0Ms{V9{-LJKk^*D|@g=7A zPdg0+1^!16mnPR4|EHFzkclqKzYN1O@Lz^u88KB_{*^Gmn8|EGqyC&q3gabmYM&Do zcjC1yzD6+q3@0FLg1^R-$6rqY!hXc?z(V|427ku=))+qEGdBM!V%*Ja?cEeS+}xcU zY`t--mx)V^iPshQb`U!V%{xr?xYN|{j!oxe4Oeec2wQBQq)D7R;)pNehd(107p02N zm=palU^6ZKu%*fLv*KIF(GU5ks{sDC3!D*eapJWUzBED|2mT&mH43hRAWP-Ip#Tm= za43O8862|UfVjcGKlx#HeIje;kR7d*&04!`b7f`T{b||730j9~PV9ogf@#?W?ZB_G z>xZellq8?GvU|7sdHQr}hIPAzr=>C#OXD1=gnvFaEtM<(K9x@Mo@t7!2P?;-E9U#H zD-WKQ%Jn}&<&0w~^!KUMxK*j`x#N;t!ugMJZE}YePD|y5-$o^j{rzQ$#i$G<%GAA9 zO%i<;WBmA;)U;Hl{5C3~*Z7yIT%&U1Ox`Cd?Z}MfO_5tRR7^|dxgVi&M!kd&?Ehsd zi_U7Op%x9wUme|WFf#9J=d@H>{x&LC{e3FkqfD&|D^nDcI96=4vvt+a0U3tSAIr%;_QmOv;sZ6Mr|@!eJWQ3OEq=8OOrh%WL;(a;oj3}sYD*3{B)ureI%d&-dEFkmX{n3_xWP1$$}_mEv47P=7M^2^jQOAdG&JJifd4fIS0&QmSON~1 zkktuuok%C&<}-a z>QWb3N+AnK zMNLcbBT#8Sz7#Sjq?3BU20>qgg4B4xWOPxeBs>IR4LSaPh8SZdSb$c_-LI;W`u)hh f`t8UT17wT;46=WQvwsNLKf_sqVo)5kF7f{W$0ilS diff --git a/Unreal/Plugins/AirSim/Content/VehicleAdv/Vehicle/VehicleAdvPawn.uasset b/Unreal/Plugins/AirSim/Content/VehicleAdv/Vehicle/VehicleAdvPawn.uasset new file mode 100644 index 0000000000000000000000000000000000000000..b58c83f280e62576208df9b80d24a5175e25dde2 GIT binary patch literal 119544 zcmcG#1z1$ww?94z0*ZumNTVQK(%lWxA{cYad?n@)uTm zq#BSo{HVuYrbK#@Fec80hLVH?NlUFp0bNF3)u4`G? zyt3hfK%XdoGHz2NX~>WHQxp(g82aL+JNoJwBPf2t2~EPla9|vu3k`$^LIv&V@_~>_ zKrAdevb-#sUz)dI;ajWmQDI#e|OPvBs zVMX3^NZP`*Y#|)FU>jRY2e71->&5NG)l|zK?4SyEu>rwC#w?ltqcKP|8s=_}pwMq* zt#|&j5stPHbGR+kQOW@Uc7oYD!a*2KR!K-3f;hA+VYW{2^O=H}Gc+NK;4;_jIm3rPnDsGAJZ6M-!Pov!1o5Fxc& zAT>jfdZiE+77kzyTSrH*708+WrV3!qzcY1hU2Ksupt}qrA~yl93fS7*(nJ1lYgTlT zM8IvJFk3j%6D8F99tN z7dRNA3x>G>CJBIFVfmX@Z4W0f=-TzzNnmfJpiqZj8-|Bg5(=2xLfsV%gV|bv#QC-|?eD+uBnJyYcY2mewvug2Ddi5wtp4*MBz%{F0xkTlYb ztf4RuHFF3U6v?N54evLi78C}zb+iT%UZ?5?{P(AG!QI?r$j^FExDAqxObQ5xvxt;4 z2OQi3G(0r64EWTqIsF{Sb+V%aEaw+l8w_zmjs$}S^68p^osdSjz@ZQsuoK)y3l4U) zv~|!mM@BhlT2iAHn7s`6zB$4H4hlGhiUE;!-ZwS9SeDJ@l~Q1CzZidID`45C(x9tX zCux5hdX@^*pjKc|#mv0xZwxIR-Lc*P)wgM*ZA1fNAT3T<}k0d&bX80Y;KVdh%}}wn1}E3vjQ$lZAwkL(P$4 zVFlXALpKAgcqX6;wge;nU(wMC0SEPKVwV89;=gcy8yys5NCa3?*6L>xM1xis1FJso zYgyWWA?Bva+@RNQ6S0A#Mb^p|nP_C7mIxqz6&>$GLA9oMhk)<=WSwVBsVeg+z;^&5 z6ef*;xq@ZP;pU(T@p-K?VaK!SYas&`40?N0Z5*)2&$Oxqh9jInGTRd3ztNxwM=Ma0 zO%(%R)N?t2_RD!d*+L`V&I|#VSoU}Ik0r)7&kXTT&Tob&EjStYIWGj{SCrfxN|pdp zv#cv}tv?N9oQ-09woL#DnMlCMtZ4}b3olyA-t0W$x{K94c=wSjzfHsmbxWIX6{JsSn2A!&^R8(@?I zGA*13u`SpIM6A5n1dLL2M23Vs*c=A(_)gvWlVbUc(g+4tspyELXgMKsB?$i(-iMzg z3-m}@H|)bvA4 z0>r>i;$P06I97;b(RAz%F~;($m~g<2sTz{n!*>`2ovhueUh z=)%m;P@t}0Ap8L52R*;Das~~|8Y+xz@MgW$Jlmdi7W(=mA01`kubD&SJ&Sot!LPBhFd z?ai&hAQ$;yBA}8z@2fy9fy3$SZ~&(3;$)7D9elV)Fkq&0pgwh|0#dF$5alOyOy~t*h<$H z?52MIz6%%*@^1eo^NVf>g+e4D$ZQ1%nG#2I0Cqb&5B>sBAhj|D_WX>jZG(VVIGWo! zoMl(KCG-lw@IO;A=(IM>4cId+X)RM=n?PLseecgS+FCoB!x1p>PeZA_L}vgNqIE$& zbJJPU8aQA+A%B+d5dqWpfx_%};$L&G z=hwtOLxO|B$nx9zr(wcIoL>Ou)`G)+d57>@7{l3A&p>~XIYBRu08DML`(F{;A_T7n z;twEdf*q}p1@Nz8RNRB61VH?|p~_Tv$^qZgMwU$4woahRWsC-32HH@VGXnf`R$E`8 zsK5+#ka24bRQ(!AFLeQt(`WDiB^t71L&E$Qqo|gffq)5Kq?xneDM;`g1vWw#`FsF{ zam#>oI32D4o_Ahi{Ppzn47K(S0Q&dZ1GG7ZoAR6d`I8T_B#E|b+yjoQ3;F-@fP<9L zO!N2(c;vav&jax+eVDRRUILbV&N<65%xnIYfHz*y&eI2|BXj>T9g?nR14eGQ)j!z3 z49=?uYV9lZ0x|>80PR3qKt2sd{JaHS@BkE`9V2<7KW~xGr9T^h1hjc1kLBm>*+VgK zd-J>x@Gj(l+yk@=-W-z0|C97z#sNINf8pu>H@vUNakBryGx-;u^?$>gMd}1R18DzT z1Hikm(Vuug)eE#g@qoYu+MjrQKUsg|A^wHu{x3YQf8qK33-7^y!<#{_J?6jRO(S`( z|KQ1<_qUO}pnvde&-=e$F6{C<@6x}FyYS0DjRRf+0qsw`3);o?cf5-@_!AGv;z0Wo z4@g`4iT4@29u`BC-K*rvCplq`EVGzmM5K(vE(k zho3Wnc=kBI@BLrEx$w&iY}WG|U@c$Im!bIjf4-ir-_`>7`$bN^jeH?^`y7+)&3}e- zA%#@=U+lK|4_er{ZlFw_K3}ZvZ#aOi3w`IJf4cy@KjEzZh6BWr)cGhNek#xJ`Ro7E z{QtrMhWsZu7ybs|6rSUJ`(MC0_-#Ev>;q>QFlPXV_dG#8Yxtk9=TA7mwg5Qn=gIEl z+y5IJU>N`o$2rd7`~Nu(@X{Ws@~i=Q132F=?51+Ap1JdXjxI;R zf^%^e{f;C0e}HpwuKkX4|Nj8z;*9(qC+dG02k;DkvT>OvuZTUCf z0P-kMdjb0i#OXzSKk*w5z`NLQ8;Ncx?bYiv!PK*O4FKc?)>*2gU%eIDls?z<+?+6Bq~3PLTIN zAJ7Hp1d92I`GqlpR%Ij0ZG4 zn=ib;FL4Leeey2bJs3?#&qv8N$))FJ=?|jPS-2mIhVqeX;rVrY3_gVv7M^DvZz;bw zP`QX6U5}y!i67;hZZ+14dAlQ<&!8msC>x#6-rDrEj`$PYgYmDm`STLY&dqTcp(l?w-qp7He|ACvd$eOm5t!+MBslrVKi z7;Se9(5H;nF-BRCu#w{K{v{h>ZHUi#ovYb1-Q z$l+TU$8}N)SKcSBBjk3+t$df6s5uhbjg=St)(uVPzL=hzXs9*MRX2p~qSo+xQav?r z+nC+oAA3iiW)OV!!>L9Rjn2&zS%PAeq4dsbzVSCQ6u0qLQuE$&M)-=;i^K+rzo*C| zyHT1Q)Z3aZdfMz(twu;IwB&pvysiCHxgY2)W$HN9bFz2#wXf*;Fc0KYQs7SfsGqr8yR{X9`vguQTLiClf;EPzO+L~n z2MKu3bjjfdnNcO?rn%jIpGq3;(HmW;ZucAqjaLk7b1LM>pK={ixbdBp+R^E5Y*V}b z&|3+;8Vd>1!|SB?qzPqSlsdO)y`UJQHx(Id%L}Y&MAcDp?PweS;*(BC!=-$A=7^h?Mi#yj)Oj0Ug77~|Q**@!`mV1` zrE`+LtY^e<3AKDH##epP;@*4H)IfVdhN6BsDS+nEEZeG=>{im9Dhulo$6DRui1qd8 z>jVY|N3Zpsl^$y!LM7B68_kx*xmU>F+9z$v%NKKvDq7`uW>|7m9-i&3K#{SRzx!^7UKw1K>#Ef0()SlRBY4~y zov{5~M{8|SUvdvlw@G3OH#i(xjxkX7LVYJH9mhP(uHV&GbcQHT$uh-)8l-RqFU% zr)9nUy{g&zD*HyRrX1t?jTPl>{`%Er?ay$^?@KqhG1;E=6C_ z`$I-AS3l;|bA?sf?pifTy-ci)@ZPU*%M(G2C8;0E*A*mtZM+{ZOF~cXd+rL+gbIlq zc=Msdm24}juN@~?^cSmahhi7=jkhY*-+*v^v3#_PC78B1w9b;u!(o}5*i$@ z>m*JUyG-VnP(*g9k@k6pEJ!TkLDhZvHE}d%c;`p9CXsfDJZ`PbM(3N;Vh)|=jKoridGLhq}STB#r%SstKzb zo~Rk`kLIz~byh!?%lq&ddx6cwIV?bJyFl8wrpnD&xYxBNsN8m!Gfbz(82YWsZ+=(w z7*f5{G`3~m2&>c+Lnwn6NeVU!S(NL23XbOc8~Vl@9}KQZlDs|bMCRZP%13Cgk9Oyz z*=SLH4lr7`f*|tj%t2~PgKWA>h5V_mrCm_tn$Npm`BqJ8-v`|DtU9ET;T)z)=N*^ls!dPhUY z`8Kk3a12Y`D8#Z6o`R11GjD79(v4TO=c&FY6yICf$St0e*Egzyz0YFDuIU?lJcL_8 z9lz{YS6I6=$i3!p`$(%ZVecq*hgyZ1(L(PxrdT(V1+DtCWys%W8mF_An(%X@nbgJAEU)0 zTN`}3h$7y_+BBnQGfk7L_S6&eKlsX?bZU?^!Vi*geV%rbR3pHhIgK8**u}8I>1ym# z!+gMLiV=t(<{AAxI=(BPCJqJzOlq_5LJdaWJ4R4tmn|J1I9oav9+V&*+1SdE&!@Tz zie22$H&M2GFz98e+-$OlK|gs#-&djsib8Ya!6Oyynp9ZPu@K*DV*A=VEzg+>k3VPs zcaJ&$$%{)vLpyoOUbvQBx-W&2hIXoI=EfQrt8F1#fe5y-9nlY7LS_#2 zyH-_&EIad%wJRUbvH{K8waFUQd z?u(@G?)56=ywv`ow-=wM#?>?eIlw4MTDh5V_j6ZV?xOTf3&}y3^d9SBdEqZNShQ+B zO8+oq_KPyv|E%}y0L7}$`t#4Jg4%I}$WLfX+!hYdHGh>hVboNF*7Ndg% zwkUF6@F+Aw4QyMjo$_3Bzq6q&YHN(%^F1f>4?Lt9)EmkfJx9v}B51F+uH3`8dQUB) zDd&m0>d4kkIuWiRqO;iXa6p`8_uD-L-}iWVzs;4|gj#u)#y8uUTRUArYDwxIh118c z97d|bg1%R2orr$uteoIR26pv_%6eyC*&Oag$T-XQ$|wiKbp24#gx9bUwCmndt?38I zF4bx1knc*N>4y!&z%f28V0DE zXRY{n`N@PYS8mr>MQY`zJgUSo*CoC1QQzW;#6h+~KBY;-D$!Hlu|gKpx@C#tYY1^< zoP$>+$2PW#y={KPvw1qF|BC}6?*QNJTvuPO~)F*%ml5gqk< z6W=&$>$mwf?KA~tNP^%BMjh{|-epl8OKpyUXX$`p09T>9|y-S*=FCY%YL zd%>NPE6{p$CF5jO`&yyG^nP}+)sKObWpm7ag_Hf4Ee{d#_SDbDC#uF8^q}HZ$$dYT zp6t*+JJ2*(Hcf{-i@;4jnGf_}i?8h)fpPSeYe_}>W+Tp*iI|Qs+?ZS&Y_Wi&762lPmq6atB~0c_p5hT4#omD3gl1g zVYsKFBQsz6Z?78a{3oiAMb-}DIPS+D4B zQXT8r>{>~%*KbFD3!hE-QfRuGERg->v-qqYc@3m-Yj>HtBSC(A$4j8MwEphhrY z+UNwV5*6Z?uWnp_FzDIUTNDwq0h>2*uv7#MvM|&KZ!M8>0 zcK6=rsU601)E`q1#p5V>SuaP*Ghod>p-ZlH!}<7Zkx33`WHwfLwcy8+Y2F>|W6_DF z_apfe$H;{FLpgb1cW&YUuGy^{UF4HnT`}&}bAQRGDy6wrKFGAbQ?rq=)}#GM0cV7o z2wrM@DBgVdEZKr+Hs$E$;H{JWJHD9s?UP4MjMo#Iow1w_o0}r8>3=rxd>OAM>WU+H z;BDx7O4XfE-_|IPTD$dms={~-db`w>#VyK}E~~QsA#7C|^-iO{Yyq_Z^w=N0RrJN~ z;`$r%Qdeq}Ey5z{jHi}M*2Z<3-#cUsp_de29+c%SDcy{*N{Vv z_ctb2p86KEf39p`#43dSSnt_tu6 z8HRM*mL3!A%ZS=8N-wx9OVzXeSTE{PSF12O_Np)RI(YoG*d2~r%3D|1R^1*mT+vzi zSiTm)t0r;~&sMx0KjFL1>{eXFDl$5Bl1|_Dfg)h`s>HFc=w9${8x287C;CHPtIeV# zHm|wWlU51qT{jBK13mJsC$I^nc&&~xp)d5hJk^Xt3XYXywUy55r)+9$B$h+*4e!VC zhwf?5>dvRG)LR!07yjsoCkb1vY}l?k7}z9VHkM~`<5sWhRSRKR=oIJj%|^^L#5Wk` zJJ|D%zAW;#U8S!c+X7Cqr;*o$2JMYcDouU1n5Df)<{N$--_&rJytH@iPVnU-i$jX7 zdOo2fw_?9+BI<*v3iX<*-2HoO4_u11i|dvl2afc5&pr>1BM=jDSB~qh27k}nPN9N{ z&kP0?d=N9zy`^92p;lnz`z>Gym)vN@L4Na#nmPwvzvW81>9+K$faCX&pbs5_M%DPT zZ|1MRb9Q>XOv1FlP@MZy2~cN(^=qXpH;-&`+Zus=nkxw&}GGBEQl)r zUKgH&T6RL-8(u2)K4*&b2Jy$hhW%ca@}=4c6{Ydq?m}%(J>2}-71NlS&L6(4$)DZO zpm|1{Lt~8HpRZKzXimn}$z!P@mhPk$R%f#fc|Sd+5)!KR9DM!x2d#{w<+q*UG_)GS z>X`Ve-A1^4U}clS6T?%PF@47E4}`o{hJlTU#nV>~_pHVmgABg0xx6n94`6Ud9`u=- zuRX3WUe$eT-Xf{bkEQ|VC#PQ zLNn(U)9PDsaMirmsu9OToX}>7H|tmZE~CxQfo#rQBSO%z_682W$sYwrAy%s;8B%mFh&|@Ur0@QF@8X7i z@{&%X3H0=1%&mN*@BzkM;@K?IIr7fJBQ|F4g`2b@S$?lduRGo>ZXkr(f7|&ihEQKF zw2rW*Kgz}K4%trmYCHQRg2S46+*I1DM`=+Ot#LiU)o$jK5%UH#&t_bSg#Sh)v@4-z zIkiu}7Fxdop#^kl`jT&qyhaP=|qDi`i$8G{z` z*V4=@iI?!I=*9a4pG=Y-5*3=p*k1KxFoOCqXsL(EpE4t#daH`Q%tK#v9+xAWt4*Az zjrVI!1>yYl`bnmv$aJ__z*|zq3K82YTiW%G3+_TzR4b3undXSxhX$7IG)55gPEEkf7bswXF*RkptvZPRotL_b^`8r8*E!74u^u zs{uk=?rfJ6o8 zk;nFe=y7W!;&igLymZtzX*ShhEoYl zBfiOvBcIEt4ZGqS5Yy`;yjzP#>KH?K<1gJbw(g{vl0Ht-Ohe|>#zyv;D7@w2%7z@R zXB&~x%K8Wh@{tLlWXde=U{U)u{=Vq zQv`n=k<$-q4o{<@S8s%2wVv^ArT5ak`6a}5s?nUnjE+UA#^UK>UM>&#hE z%u<&sb`8qz&#s zIj4%z=^Dj#OZm@R*~@QR+D3!)-WA>z9pyD7j?QE?o|z!x7xxedY`cFePCbc!IrLqk z?MbM&%8f$;$(hJKf9(;a8BD8%d62loSfXl-m4@~m(_vAcsLFOcRO{T3;yZM%>Je|r z2AW7oy}cf-^|zMCP&IkXaW|Z58h+7!BLm7L3H+L!aES z8AZeBT>FAP93lI3cPdP>PW^iH03IsdLWZG4bjD)VU~hE#AXI^{zwerQ_^qf8_!F+{ zx!1g7gSV7&i9$^p(Jwp1s3gA;qn%#Urnpzd*Pkb*J76p5=27`n;v(*d7|4;7swEia;_7|5>tQt5#OM8>KAzfIAdr(?C-58J zn~L!ePq0c5!>y<3gptY>!5AD7EvghFHqTw!Q(fpc8NLWAg|NhP+0N{AG|*dS3?uC9 z#^QQqR-V_Hb_o(EF|s+SlB3$WeIm(pDRCz3Iy$%yzWerMXQ?3$$w@sqnB*P5Ec zL=xb6>kF-e;;dvo>c`6ghJ=H;1u+&AQ`#qU?e5v(%W8vhTMlR_k2X!nd_uGvW48-O z_tr9ql9h*z>I54JdR^E@wMlgkZ%N&@yyo$ZZ6~EkD(zj> zRd3U956&#nuN2ABCy$?dv&J4%VeGVenT_h8&A8}5`uJMwHW}*_xtjBLz0=1PCwUVnVWOs8R(VXm@Is%9jzgF@|7*m)U%ut3 zqnV>nDLu7QW|eW2DW~IsK8egsQLpRL1E|L*2Z>~_O51or>T06x_UapgG7H@Et^722 zX;ZBgnW!kwJ(=>p59?%i78l<;d}QanB{#0RF8p~*m?U?SC1jctA|M?ZY8=a5@s1Fi z-If70l^_lhp*8e~L$QNv2=*YWag0nM*a_<2TEQiP+>gs;cTMgDITF9I*}3X*jHb&*P#F7eSeTe+_`ue$x=>^PZOb+-C0D#|qxpct#^V-3 ztgscXX1#cGw_^#RVo>y>j0Y$wP!ANgoaueZ4)@QJ1s0U$C(|c4@g!b;^_IcdB~Xg9 zd$lfvns$Yu2SR?Tp#`~)ClhRJm;QtcK`rC@&eBUL9tAZ9_iEu_R&dMLH7(8q3(F`u zi~;71;UbSUovTAtAq&&(=E>_X@9Z2$J6rMJ2g`qRs^)GHw=0r$!;viy-6IxtkloX% zMI(!hG{lD`1rmkyJlR6!`(PIBZ>F-EZ0#P)59QS7Ek%#lErCdj5K|M1TB(zX<1|h! zZ5s2f-LOq*H)<@ixf-^D(_r6zMFuVFrnCuXtl%wjEF$BMjdzXOZ!yL;HKZzboB4_J z2`#fcw%&Rc``@W6R|_Mo`f6Xh(~DA3If|pi&~`^b`?BbB=lP{p+r&Y0Sa<@4^^2ZM zoh6D*kKOZw;r9XY=1OQ+}DrqzbZ*b86jF`sQMRe7lZv8~*9y$nWQdl_ug zo7XkDyjDt2Wfc`aX65rTbi^{%6dj|x!CYl)28U3agqU9cKqlomILOG1MbOM3(O00Y zT2_+GIE&et^BezgRlviBa?kyup0M7IdYXD-5&kx)-{rQ9AP$tFwE+_J1S$IYn1GiS z`+1gnl!Z2JM7fJsZ6`%&yP~?Z6JL@Bjn-2+jEIC z?5bmW@`)n?<@WILG(3{FtNsVOTgSptV%*WxVjq+xg*G`wipMA9Frq9+v?0^KoNf2)#WA}W#MXHEs&e0u!aee2yE`v1^ zHst8>fFMpx+u)L&oo+5Ef5C7P{&EWsCw}x}66S-0G@hzy+P3SFh)yl>z752_wcPwut=sln3C0B7jh&Y0 zgs$s>Jl>iZYo8fAwv^Dz`PAx2m6$f7^ekYq%~|w(SGVSS_(TQnH*Jv-h|DX&RJ#L{ z@pwWkuvEmJJ+aI6P%Cp%vD!g_RvDKTsxWWe-nk1`L;ZNyq4Vl%R+S$5E7x8($JEVF zg!2m!vM=3scr|I7LRo#ql4VnxW-9vnM{(bF`~#X%RA_E+41>4?tFWINh1+|O65QA0}md@7s_2yC8w-5kEUkO2BmAB#>*Bp8?HYn zqhw+d3t^z14vQ4ZFC2t-YL#E+y6?0d)7Q02B8uky8RymeuN{xX*KVsw{dftNd1a2a z)j^}0Q3ImTXwOs|Nl)k1fcV(UV8XLpZ^_oJ* zk`?Ww#NF#O?7rn;T8FrrcxiZu2)yx}1V29$&8_=2H6j8_(7KJpcw^p9ldk!LdMjG!RmNL4 zsB?$(AWuQPZCBnu7O4?sPkpo$7jopjt-~R)ciY+LOTvl#Mh3o5nnG;o;tLlcVm>zV zz7I8ot=I-F?*bgdU8f^bqw&gjQr^jb@1>faS4F5{+N5Gpms54egsEq|HoZ0y<{}i+ zC>Fh&(-uWd$x09_NpXtpNDv3Q1XD@8JA3!;wO3%l*BpVvAJC>ZBDwW2;VnGG0uCZ{ zqTB)*ao$ui5wwJqpxPIVIB)2d64vzJksl_-g6M;|O!1X^uy90au9V4&eAT?qgfZ!B z)~`nq!UBp-P;ps#L?`87X5MMuuFDp6v&iI#C&MJ#|K?jhb2QkJiF-GSYs*&~lQby& z9X0jKvP7K8$`!3ey+Q2EP@5zvVoU*fvdQ5+o|<=poN~SUB##T3PQnr1a%c@Cdg0I2 z7YQpqsJk(!i6`!TV3~9*i*A$NSdeY?)*FK#wMDHbdCU=v(ibyIy_C|caPO#MYT@pEW7*w=kbq&zpzqUVw&TKyap!nGN8Zp7_+{)RI3kf*&Ag}s6761MVm(2qev1?TW$EBQ-` z`wLf612)Lo^h5eOI5T8)Mt7xf6z|PVkxlCGEVaMr+RHcITI(0YOZMYS<$2boCscpc z)kO5}BY8Fy*jJw0Z@O6s)MjVI1UA$l)C{=yH8ao~%yHj5O5|m|#hw#XT$M>&1)Yq3 z$~xm{Xnb%$i#zxB;m*eK2*G-=>Fe+q5+Z>rbo_jw&CbY$(os=+R}^8B{pA9MwZZP2 zMMpa!vN9M}ug$vA%&w2pZ`YIME;yUVILijSBd438+tHhBZRNNTYPzA@fT3U3_f8MS z{Ma%8JGn$;SM;Qe)+|hL0`-z?fBFqU!qr(upJ*b1lldfKD@_yl9hebA+lz!rDpWLQ zoJN0VlP9R^B_Tt~rYb~MOnWLXzt!3;s0wuBO7$(Y?6mkZd#kGv3X?tCi;c~Ah=@Ww zx$+P%I$r*&1M#rFI7y3u>eMBgN@}0?={wgz?fPZq?E%@(gW_RN{36xMg zUG2jX<$TGEG%nlkEu1I2K?~3xx$PF|p@Tk#PwXnziwtT!C~uC+UZB$n20n(C#@y?* z%xvOiTTgtrm>*4&dYqTsul}W#HlMqpPy6~Ctjg`1Ti>oEdsENg3%Jk~WAmI~JtDX! zC#D*y_I2<5Lko-8O3$>_39R?lUgqxHp(~0l(u|WX%F-U|N70Uq)z3qWHol^qmP>%u z(fsgYRXJ{jUtP%b6=HOHh*=lX0WEvZ5Hh-gu$1hc%siaJJG%Kg&%*HqGtEwYsE5?2+rHL+#0xx9fpV!P>{O*-DZm-GF|O4wiZautm~ zf9pLn#Lt<=9Uqs+{;3oXZ02dV8HFdaG~yqmtwSW}>`hp7)WUAM(s$BsmfpcWD8)+X zMD&i|+&O+bc^waHGhwZnhJ*muIXQ$MZ6oA;L}IMT(ZJ))=GEH`_to$*u%I<~RNwJv zs;EPfOM4UxYv?#4CTA7Gcbe_=C5*X+I(y5D+-GWnsYqyTC+3$!3baBZKs8chyxvpt z>j{zdB&|Ymd()39A3qh4@Le?XzAErdcEG^0-O3^HjWN^%M~}@;0%UBqWyMbcU5uGx zsGvHXO)Hk;WX`?;ej@ShHen^o!-B^oDNW_hX{f4`*dnr_lC)gc=pfVz=({xESu|pN z5-J%&;vAc1FWDVrJ&Ypxz!*k`xj5YZz=F}r`TFaMWHSv?a>QNYPv+ODwMo6Tsp?7X z^gKV@Ugty%KM_TLp#0i}ru8u+b=)NnhX^cI(311~^oyMSvXVQ_U!{*DDQ%Zi8Ck`y z2l)yl^?rPYs#DgMOOxb0^%jTMbVi9I4i5$5qh9)Mi~jzG)+uLjy)~bBhn)`fy3*LA z%l=&Irh?4|t=8nz2ex7z&mI_uuExC%k(|a7Pm=8meX+L@^gMdmXz{HC_2ERCh;L@b zRg)r#hc2(1_HqLuBuia;ZdOE$Vpdj!L9Iqt`e$@SZuxreeN)bn9*hV-?qz^#@x|=D zwxC5!;N;$UdocUrw$I`Xc9i#tq3sW%d<4_Pd^vHBrizD6SaQD*3lJ714t#EFT*WXD zF!SQsn5Qrp8=BUUOBECK;S)$9Y(gixrUw#Xnrd0*$#vCJ=MT^%y>|<=LFvQ;Y75GRYVI;zz$3r?3^42v$hy6=vb*!)h5A?HKzHkOzk)Oo5)aq(q%s_+b5+ zy5h(B96k}NSK@}Dk3y!frR=J5gj}!jbG1{8s?2B(JCEiE9S6tgdww=2=!3f}0lodkMitj*7iTUmOz3=$+mnSeFDk>XaltR?{WZigG;1=;a zZ8cKs`Wwv=#oQ7GviUsY$qzr^s(ujMEcf7AYQN8?2Y!{_VO3X_fBf-Y$CtO*m8_|y z#E~U0^f;ek$=y<{W=udggDI^5$*&b|AM4)k~-YUnB#;}8Z zmYv3Yruup8!Htr2947tJWmOPSkL2P3=iV6Z?$q)YLvh&G z?7;}F88;J&clIMHXa8wT2R*i#PeBn()oKznaYlIe7DCukrXKoSZ^SKANb@n9l64cw z-*%gDh(bBpyZvT1VRZ!8VWrKQOw`^ylV*Ocdu#8Tc~T24#qhHk)sZlVBjQ^gx1b3H z>H@dP`RexVXnEG-aOdxW3hdXTBk9YrOyO&EE_FI+$q`be^UIIgyJw8pN8n`WrK4IK zOW2nW%G}tO%WjxgvhhW zsz9IqM88nV-FB2mih0e2B%4Ex;=b<5D{dC# zk@vEyd<~{Yh2QfX^SVLntHZOu1(xEwrl4u@uK89TW&ciUM{^oXj{aV%`OTU4nFk|j zOm|D-@WWQ~h^I4MLEc+0#R+)$o`{u zWYK(BkM=QRNuuTx6(XPR_qtsR4*L2aXu@Fe9X|FcdRWAx3IhJhVm>SSYV%L`i^S|U z_p)wqW{YvTeQfsd9zc<@4`#~<_=wvutM|>O|7d4*nnN_szv-Rg49dg0Y5QKTP#to7>Q@$;=BSCttOtcXL1ZiG@^E4sXCOz)FvWeUmM-FqD>) zXN(WXQ*xZV(9{=z%^@;1SGGuY9cA|bK$;mVedcS5uJJO-FXoayvl zb{(>y434qjPFk`~@p$gv8Sq@P`Q|#CSHAADpequFoC{O)|8 z2*+9YW=mZ869yTAT$MHcvKiz27`5Z^Q3?DI+xOC&#OM(ONJI2bWNV!h1H(PPa~TOU=Xe z6L*xaX3d6JGcvagwDqesKZoXcMZ@grM8VQAXG_IqU=~`R)*Z9!HN#aZwR+|D%&2@ zaVoj)l9%_=79XAzQf&G}K`k--U>)_Y;xwP74bg!1RHm|d^mq=cEm~=qc-bqS^2qK% z4-P}~qTGfh){WzL`~?r*Yg?_Y`+ZO_J+UuYi;pTM*GEZa!rx*eaBe5|^^mmJ^ww_l zg;3LaK1dV3G}7)8zphHa3$_>}>#CVjVfRaHdM1ok_>s_Q#08PWIo3vPk%^}a!Dq78 zYe=5a4SbOviB^rq+6(P5&5)NOmC*SbG}=606u2QVEwd|Wt!o;ex5`5KNt7zzXJ-&q z4BGez-H6XW>iUL^7Gj5Eo%^M;RR(_Q?H-*LMh*pI#YOHHaVjb4i&xdX=Apxdvn|qs z4-|JjTdE$a-11hlF>SO%O+DH};Vjj?yD?mm^0m>fd?-x9`zC!5XB4M!GOx`n2&?UW z>od~J_{>7KYU*!13x~q$xJyrRlb-s#v?3BrSCJ`k2tr$~N4xpNDq7F!wrXVGJT`N~ z9Jk^J15l;METIF&Djp~^Aksl>xhJF1JGC4k*ID=Nq`hpaCW}H{zrt*>ecX!*&ldkm zPg1xC9EZm-$Ul`-&cvAsV>!Em)=8b`n}xJ%=_-cePGFtlqo>z~^YUcOv}Wjz1KAs8 zH{7|GG~qf7UOeimbg5PEWVFY`_ZTScQKVjdV4W=>o|P_E>P@&yoKr+W1!v}Gm*0x; zsj&EXDTDT%$aKrp`yB7R52WwED)>639vtL@+l@7*u&9^_7$YscTVSAYiZG3u{%m#% z^dFdeEZWJK;>@O5pA2+SiBiXPc;rqRsPmncyc`B^W+UEXFQ_YBQ6lo_a4!XUl;K8T zluxEME^~{x@5xT&p@8k*jGJ-ZqI-`{HYL!6zM^VMq2!HB3RS1ayACZn3~Z0CsB+T9 zCdu#TVS~05uU%P4Fw?H1#VZZvzp@^H6=Ik^B4|a5c?-@*fN{U)oz~8s_N-=1+~PM+ zhsybgKDD@fWYwMS!({D#J>V3<=t0H3v7h!MB`}MgfaHB<3&-JY^fzLg21+NxDGyi4 ztwFJLlWo-R(iSc^%``*3Ek9*GX?`%vx-H2+G4ZPP3h}G?%PVpXF*z!pvGH9x-x=d? zFT`C5LY=yV=lzl$qLtO-5+t827+Zdw!ueZ;mB<{~xB`$4e($g;evUfV7wRJ~q95IH%~Hl{^r;n)iS;kxNXRjWMaRaHXA_V>SqpR!xx!6bP1~yvvP=+5u^6&1RG(~plHgXx zea=pVAbR^gZNtfNh!o;%wHyt?W?*0MB38+INZ;5h6^!vHp;_EDXNa@sJVo*7N#mUwp@W zoJ$P;!IDp$h^ex0pSPr7Hd~>B*Ig1mo;2$-WKJUXkH$^&XDzv7Q7Yd_2`xReoglqg zPP1^|REP z2WvzH4%U_m@ZjDB77Y%98B6CM0vHRqGfzlf^Or8Y+tu&DooBM%vWDavP#3?!{uC#h zJYr1BgYk@=fZfHw?0|&wl1NzF7vI~NDQRo;@5(W3YA$zeQ{bwGEE}wg*n5v8w7WXa z5xjiL`TqeyK)%0uOX7_8Rq||JxdqSs`}*a}!xOIpAA?KKhGWNZ$H6ym{ALxGDI}K_ z4?Slz^>r4c&qQMw_`Wk?;2<;(9ObT)gkTmP_hTRRLK1~rk^re=KJw;3K!T3^(?nuW ze4mI8Hd*AtoZUEfZP_~3S0XeOT(aQQ+>>Lx_7#ESL{v!po56J=)y}3Rx^`sFxE4Ym zlT(GJ{p#hb28d<+?Kb}DOvf4P zH@&EWzkIUTu$mGdLT7YHUB|>a$`B!Zn+IVyV`C)p>qlp)D*nHh&E%MSOK0s3qUZXKv%O_~pd# zEFh)0MZ*Z{XLIJ(gtI%C$YhhsChIFP7AAE0?cX<08`ryLwjKPWFgv)7ONozZ!@-JZ z`sdpszMj)KIvpai3H=PvI>PNy4@I`U*ja+|$7g<>UGT}sIv@?t zYkVNMG~_Ilh4aa8^)`fBi3Lz@Ddd$g*OxtVmOSIP&MNE>Cb4^SiLObYvse}|7z34- zJB$xGKQ=S`)fk2*5fbVAjxO#b>E>IpV8z4S5JD@;27~s7t0IoyP-+;qV~7%e@Pb~&;w|lNY&5PQ0)0?(RoKcKH8AF*~4|9=S&JNP_(c55{)@O$yM-dAhsyO zPM&KNG05~1DwmJy^18B}FmJrUVailzqQoDORN2UUrJzOTD-1C_xqT0p#S@<~9l)MF zFyV=3G$M;S{=st^%FAdFfA`t;R;`Acp?DbV27G4Jg;TGYrA#~$2c_v(Jk08F< znEK5}`^B1xMn-)(PEKW$hk`d#O8UIo{P^*+7X#`Y7i=(*Toga1@SSk|A{GtNNey$O z#WEuLY;B&N`>?U;?8c=m8>m67uLDUGfgi^uL$F+ki;idX@DXQKQph7R(0-dXj+Q1e z*SGI{s~{afz|kisHsV)DV*t5cSm&DqP(6nP?Gvl)kI8 z8K0>A(nAO5^t`E`Y~llUIm-%NT_;g`B&~qP6~Aw&c~MzD3omhC9K0!-Xk58`(ANz> z5y#<9MaF076zq71MphjomZ{vZxccHuCXJ>Ey2LAfa;n+N(|iARU4U~26Q)FSMoP`R zX5c&_aMBOA$UBx(yy?nFHElffaW(|QdR`sk^ZX$fb;ifVWMj;!bA+iz=5vW7 zjsGGMlS2eyHi2yJ{PrXd{y5X-D%SI8fV1FOS~^9|nRe$EPuRG>Ya?!Mz~U;jg@>{t zEZ^}}e7Rz$R6XlOtf!B^>3Ia>A%9e0vvYB^+0xq{`ps`11#mwW@nsa9A}WSa7Y%5n~}&Zz!`X3B&hA=p36r)0Fx&P zQoE5bJLzsRGZJJ9S(423!5$MTi-rXAz=cGC$}~clt`0M?xXO6)Z@Q7-T31ceWd=rV zPlBORvG|FZiQ9*GC^|CvLh)kJ}|6|>J?jynEo zi;;jnIHiLZHFz^8sJjp#v$(K{xPS9fn0^~kKJqqVZxQ{HnMT%|ls8^1fh&J)E1QFT z!?a5Bkv4YOWa66UlbAd|L6il}|vv;tV)bMi6X@WD_8bY{D{T4z?P; zMC_hyFu#9Z;+f?n&wP5;)dY(TC>-FMqUXOd2Fx|G26kV}nc&pNKdCs^=njG_4zWc% z9bo9BEKte7L>yw@S$;#0#jq->!jQeYNB>*!V@wOJ08GVfq#o!&hqJ z<2OI{p}~wdH(kkcCglYvo}F+SW4tXRY>%aUwlAt#hxA;0b<~b(c21mYp~=MRxU1PA ztB48ET}GBm{NHrdp}c(5rT%9<-=H{)#AcA!hfiPiDliOBEa`Sf4=vMU(|o_8;5ZFs z_W~_&g^h!p4HT>{HJE@*ViMw-i7gEO-Z@W{?7Y$}0bhOKsJ@<><$)`{w7!`z_<$kn z(xk1VtJ0|^E~tsk28}O13n`gABqO3>cmMz(07*naRJ3oBw>0V!MtX;zK0i#E*ou&uh9G>ITg-Y;_5>X>g$KD{^JH6@^B0~_`U*R zQjrI01oh98R<^y_s2&$lbIGRBwpC)yW2CbbuK^o}jgcqOJOtzx?3DEwvMJz~3f_`q zVmgY5d6tq`Vk9!;7hUER!1SA6Zv;FbrGO_M_P7%Dhi?K%6Bt?;8EI}gvN1nu-uP;h z+Y8A^D2@{pV+f6+?tF2VJ_Ht8Z#8U{|Mjf6@RNcuKxb}@7{#D1@ho4exmZ6R84$* z| zd9OZ;e#RgceP!F6a2w(>>G5I@MfT$_U9yJA-Z-;C<7bq+p?7YG;ap%VX~~1)FaAB7 z=Z7y}|9p7xonP#L!kW!IP=g#ggIIsfuLJW`6odIbKZBQDEDlQBNw&v~g;ev)7MV=h zt+;w)7Y<~ ze)}37j%lUg=Nq1sij%O3R`t2iB|i!tCnS(#p+d)3RYwt%0Y|4}K#sUS$G80Pl`JCr zi88VVP^ZQQ5VprynAk;*wUZX(8+*tkJMh??xnEZ<7(>ZNS2@;gqY#_NUQ6D;S>VQ7jl-h776uH4O2HkRKzziTB@7#D4xE>Ud&6lN#d?k4stX>Z1wm zxZ;Ru?9N&dY3sJcES$l9;PUvDvlzsguOR~?55l?v{ENpV<4|ZsC0ywUitUzel5Rstx(wX~p4NfQOk#j^C zGzW#Wfs;#-U6Ap75Mj}9an@8#Nf4~4`R!JGR^Mtm_yrz1m=WeTk1byhe#yy(htF2m zhQKW&!BYUqDpE%@&iKBUXM7RB8PO1De#W9I5zw7}^UAvPJdhe}-wGYZ1(r@J;?#a zzU+Pb`q#s^5A*GW={jhxVBJ1S=GYK2g=I*K74H};d#NT@6(V7mcIPmBFD4(XVmy99 z$_n;d2TcMm3@zL*dL5SE25s>}K>TG93m=obJ3^C^6B{|o%W-+>OqRtbU-pq5$*OW* z`Y1O?o*N?JKTMw1TSVp(C-TH3pM>$KuMb)Lc}WRpByB-4eX{yiq_{)UAd2F!=?5R2?*o1()87N$_Sgg-_0?OjX4VUqwfgfHtT*qOO z2eI&?^m0BJgC_pzi`sH91CJjh7wG}9B0hyDL2>w+koT(DO%){nW`Lmqa3pH5^HmcG z8xFNi;7oiNQ^F0c{puv)(K`%C6rjl9JNOXO#&4djeASruD9(L#48`E8F*coFa;w*AfBRkT&tFIq zn+^T^o0vv3fKOfn;Xe;Pu|o1mZ_Ih_do3s@s{LScuKdX3;j`o8DC`4t=i+m;`$&O>fltYjy z{%Vf1gKd%Ab4N~4c$*-3I?FQx|8~~gH`-@QoYw2gM!P{Z;bjTME?{T_ukEQp=Lk#)d$cvpuw z#n$9ctmd5KaTDJs+=5dVBP0LGsr=@ty!`Y-V98)VSajy> z=m6rSPa^ac>x=kI3pjqrXNx*WbsP=_0X`~TNal+$zK1hPR4&t>dGn-)hHU&?;W=;M z4Nk`~b5Ex1Z{WL0Stlt8I<|G6#mh4^eTC?R#yZFR0jF`?a_EE~8?s2hzJ%pme$nsp zbCm?|%b)*v_%blnH$zWr+3Z>L_(>dyf$fL9>R8Qv{Nz?r>v-bB8S5}_G#*c%^(LSn zv`K(T*#d|cYY75^|H_s)lfo7g_0m3Muj7T##wGa zMDZmpiw0<&1@p@)IaBZ?ThmgLQ#=whn-9L^7ds|Namh#ToVb%gh_=L zpcnDsvoxg7pX8$WEiop`H%$!P>Tq`K24De+(`Vx8rpd&2~G34Zo7Q&Wiar@z$YTxn*A85?vj1W2F;Fk0lST6_c zDLEFF0>6oO6aZXQOP<0&T2^f~9k6Km-j9}as%7`o_u!kyg5VZ|UnJps^PG{9JI-+6 zi{PsiPeLZXO?XjyBG0`beqaj?7h+KZAB-}LROUFJ5t1M=)mg`dvSKRTzKEvr@qRA| z{=_w}s`8$GH%^0CpFBy(m~2vKyVK6T5s-ZxzJpNPWz!BQakz3r(85yBhR?+9I$-Pt zd%7^80Lxo$+$z0tyJQ93I(_*EtSMSFer}J2u|q7lg|BLRY@_UPWNsZ_<)LL<>UhK{ z>~KyEQ0<^C%5TYe!4oo$FE%6ToNML&`RAXqKVP#-3?8m^+0eF+0JxpGYFjV|8$3Ki z&)CSTc=3-++9FQhhUn_n>M6g8W(xw*^yg_;!ct8UPX+;ZKr+om=rQk^?RPa(xnghj#dBgVrPTRgY zDWpzv#CCtIefM4y$=Y=faS11T^mM81-V&3mb0EYmxCDmn++MJ{SmfR=0I%EUwF;- zFY(8|Bxe0e3qc%Fesp9zp)mWPl7(CjgFB0r2YjL<@$6?O@Gab6!ziL@}Uh#zuudlHw*~Lvf`HsQ>hZWJ1)z^e=)FKo$iIv&=)`m?&Z8 z2rxzklDDFA3Fb@CF*-x(iH;qg^=L?sSBzj|HZF2z31yyB6S=gawE z!7?1W_-PXp9}as1B8{FD!*f~@n|0SXP-MICx)Gnv^y!n{1Mq~jV+^zeJkm0*MS~$5 zzy=1G=<(h`@7EzmLPB=a8lK?lX0e{pv#=aEviUc983Y@Z9V4msKBo?NO)wJ`pS>|Yfg>FMS<~x1Sjn0`@-;evf={)Umavfs^R)UuGh9%kW&)TrW=L!X} zCJ3AMSI3?$(2Sm})&H3V`OiDJPXU6=XJGBu;2Lv$$(hR$2U9Y~<+{jx&FYtZbWcfb zT=Hdq@PiM)Dx8iy&RWbfWBh8gnNFsBV!R@Md<{ldA4Y~>a4|*~U|bW4Im5=Pe2l$B zCtYGUh7Q8_OG%0kc?y3N##bkjLrfkk&pcrMo7z@%MNW?e=>?=YVn68c+o{6Clziw* zUMSix$>BfwWWI2Arp#<}n?p7!HNYho#Dfdo>_v9Rf)ZgIlQ%DZ$$k2yv%TIPBp*>p zjOHs|hb&Br(z>a&Cs4v&{{c)L(da;FAKkO8Zqp351BbnJxk9{c;SN&7OyhreFR+8Z_ep&BDg3% z z6sS5?uW0HUl8!!{lbGJ*kA+}h;ZYdV5{`w0GKDxS zFYBI}(k)$ju_amLi)&Ts;m5?sNV5G%$Uph5!0`o&Zu{UrefsU;H{Fu>S7jZ5jaidT zwp#(YK3p^^a#La^K0%`II3_0JLQHXfCxrMXx?<^~uJ0ZX<$#lhud;+=8DL2dbYgLe z^o&Q}IDGo_yT5tJq_CKBg5CH+{%Aw%*n4?ErsxvhbDZ|%Bb!_OpP0n%hOX&hAB<lqETTu=uyN*< z2pP5=F*BLPAxaU^_uuW8G|E8-Qxhg8eLuq>7=%fPF8(ROKC9R#(TC@=4siN#_Vb%t zJX>HgI@4hii*fCzU@^!?IQ`)lOODOO8)JOrzeRg_`4>HK($iPnHd%%^!JL7T6%~R= zaX4~{+P*MS>M;qrJS;6A7%UHyoG1{G3b}Sk9efn!zjS$4 z@u)WenRCDC7iWxO`x3#t+adVKTq`8Lxg>_A7GKN9cZ$OxZ*H7@vAl81XILC;S6@#a zzo%h+#$6r?j$fIz4sk$JKO2EwZt`y?>I+)U8=p+{5c10xNrCM=DgYT^-p_C)<$nG$ zCpU>^Q^rkfLhm|^pM55VYndT9rTHiSJ}9r`nvDuwQ2-JT+ZLO#fd`2bP2P=p;;R!6 zlo>i`5bdm*(D@K7OPbdqxpYl%641i**qeqWfvX)o`V4^NR9FC>gqoaPu;J(^o3At3 z0l_&E;i7Cav4|@aHSkHnviSTyv}y;BGZvJjbUJrv5V{93v&{G`6mMR_BdH88XQJ%5 zf-nMfg-R|Szdih^ugE?s7TpP`NM3%kk;K;c?2%@3P@fz~mhYpfy9EiCDFld_)V!8vmyVEnLgY?JRs9lw$Fk79+7xcw5?JF_JfQ5hkW&Px+V!pt)_(SZjmuU!M6*-;!_@3FH*}g&WhickPIV-AoIJ z%q!xH0AHb@$Ym}57)1PZs5ED^sOfuAc%mA{2R|$YO8F-r&a=7{Z|d;IR(Xs8F#J7z zzS7NW!>kec1w$@fbJ$x&Z;oK9*k6wakDOC@KO!;}vf~OX{p3otiN~QbW~fU9^Uus9 zeLSbs_ngKzY$=f-pS}3+ z{CSMXA3S}8+xT$USA@2Cgn$?N7w=-Yiys%_Q5PkmU8u0UNXe06_M!xP)V!fb9SZ5u zn4r#8Q5b3_B6Xb$&kW(}g!8Ng^F?1dBEjEG@NWiMP4i3o$yxNhro{7QI@kcm6HKR= z%m$|Up|M0F@g!clp5Jx~n^06dFFdl&r8jSpy%kCpJo{!*jinTk#y|eT5ipDof6@4f ziLcS|A|$Ck`%f}FkpSjepGmD^U+krHNHRnw0|wx8)h#0k=FH7v5QjzH9C(7Eh?w?p zOCl{IoETS%ZWMJC4MEZXSv7s);=?y(ym;1O zbKZ3Afp7TPS8YR=BJ2?~bk$)~84Q@=4nG zN!&`uk4;d}cV0mEnEJqX&*p*di5c1YZefuq<>dKhv= zhEQ_kOMkP|&y}7PBO-GCCSK=3uzAknv!S1bNM3%CyN~?YE$I*@KKU#RTeRdk6O?)3 z@rKGcpo5&}i&{|-JyGyu?RBZkkzDR1DM?@I~%s7J@&!&*_nn* zZ1A665Ip*xRO&@vy2A5-B2U7#Te}D%wk5MR3r{wDmk@Tl>k3HNdVnH3$WT7%SwM>V z{FPbR=TA33>yKpornB3p&wo=+$c&Rq&$eOaYj69v#ITS|E zLa~lnF6;HDs&((1iIhp{|G^iBbyhc$@F}@UA#TaZAiiNC3j#b5^}!cziTuC$(D%fZ zkMPl`TZAskNATc?jtFeepNXN1e$>Q`EVZX6JBPHeV+;v;jJPV$zfVv4-2wja2QeX_ ze~g1FSIsCxn=UaVUE$atn6!! zhaTCEAM@goJyAJww(%1Y4CDLJeSG-a-~KMYpL}(rD*!>TFr6U*$Pj-4&R^#!01uvK z===?gGkKEZiLM`5?R!|viAVmFT^&;Vl+zP1N%SPcD`vkJgv1?FCMo_LivSUr`sa;6 zxk8k}RYG{Ngt%>V{6G~MleP+aNxu@@%>K+7KP}Z=xTC_Yp*A_Xo{vAoac)Gho;*#S<&m$B(agx{{Nu z`8syvM#(fya&QJ6LJo=bjD9BbfL-@(VYXZDXrB^E=~2pauNWq3*A?5Ugug*nhwke zAQ)oaRYAul#rOnKrLkiBwq(9Q%bWozLcY2J3S1HhM|t8e0b}sXUwZjZIplxr)_=Uh z#L>XnQ(l}1AaE%PLdoY33)(>kCE6Ghvx2U(lTFatac?H2j9PjYprcP#rONZ8 zo5^t`BA5OBy2L>;GJ*U;CtY)5}z|@Gr(uSN8-Nek0$ck>2XRG z-s_|OBNY~_Vj-LO;SGb!GoDEcXUxb-9u(pW|E-ye+RX`**TGUN5Ajwc<=g%;CYT{k zzoyFLn)vRAE!fvXQQ}-9;x@Rv1k?8&WY#w91Gp4&D_cx{p478JI3D>zeE#)WiQx-* z_QX=3U$4&wDL=J12{>1Y2um8b8CSU)Q?cg%hBg383lbBZRPEn%A=;A{A16>@3fp_4o~<}x4}soX>3D7!0s1NuOaUJ&510JywY zvl{@HX*~F1@qyXP|6KA53%vpg+duPg4Z3|5mBmORIOB0C&(Pqn)y3m49(7ST;V=KU zCp+5{@QzKw;4t8hx2ci9;fL~&0V8)Fe&>nR!ZVF?FnOq!CuZI#WJiG{ZZ+}GW%5g# zjfY>VVMy?Sym9~ite0(CJTCInxe|omy3?<4^YE3Daif*Z2P|Y;UO%z)L|H_}4144% zY=ZMTm-jUIF!Zaw* z*;ze7oCyXZi>?pw@}9lGaE!rI?I+j*0+qr5;WWY7k`u$^Hq&;zapA8o7nw-e5VTzg zlL1zsfD9=2R^Rnij{hHZk1m5}tg?@F8~3e-Lci_MLWong~Zmk;^|b~Gm0p2rA&F3*WidsuWN zm8=O*xikzLpZ=)GXPxnQxYZ}+_1U!#!kCf`!45h$TI3}@X8|+bOiFA}nl}hgiNj}D z$$48bh{noeJXoKr9gG|mw^HQAATiR?%|NP}xVB``Zk(V5UOWl&U|N~gclLkjK^K1} z`)`V!6A)JrnQ-JCopbYZ%Ho3YgoOa5wcW*uSs!)?n-OqV;#mvP%NP!bRDr-IALjhA zUIl??tTalXlP|7->^1^81Cv6MTFj$EjL#4M_K*LfwqleBQsbe0M+%H5{=gv4=j<+; zCY4u$CNha9nJ5y3U>#V3jbW(V&m`UQXSP1?;>OUVSE4l}PUi8)E%ZRmONGho$OdDw zkxQjG?qFG1!sd*R2>0y7`&knjzQ0aHMhlBUwhq@h@}3Qj``W}JPsyL?_{jenoaAu( zgTAztF>%HBQLVVhX7l^s{>2+0%M4i-Iykn8Bp52*=!}mxlLcRrGxWgfRSSH0_D?o* z<{2ExQaXw9bsw+Ms)*F=MB{97*%WwbDQ}y#~AX>m8 zd|su*etfdJMTpmyh+mVCEIJ3+0)mg*YSGCd^G7{oS$-J@)iEO+PBP3rD``xWV@4W2 zQ2MZ`7iHpq~4dPC=0`D9u9_T*F04lqj ztSGr+U}M7>ee{oU#(r*5;02r5SW|~janU#g3*9FPDCmQkA)nYH>KAMgVY1t4=-8i1 z{?b=uIs)+e@Yk0=b=J=x7GV%9jSaXG=fQA&_Wzf{Sqhw}Bgk^jkm4*_1kJ+tw*eBw zBs`MB#3f-E@Y72cH9GUuRmwcfm&T-9vfjD9b7)hM9)+3*KkNtIrSZwT5cab%@!;{% zn*bg%u=$;8|DSVyrySb=$s2;gNBx-=ULhbAe&diVxDXqg00WQ#p{7_}ey|X-QOI|- z|C|5tKRo<*fBer6|M?&PlltK6pEnG&a&}3|vsrqwChXUu0>+Z2y^nm2Vk6*CR=v+;lEPH2wOM> zzig6Drr+@((}Bqs=B>Q&x?l|AtF-bDO|^xi4I~>1V-&;9z2~m@l-&9XWgd3KTcZY3 zbh7ea|I7a=6OPO|>3D$QO$Ns)t~pKk416v^1__hA%83KMgzMQ${^24rfblTK=VAId zuRag3h?dFc%U^X^{FLmhT5u%66OzFc6J5zW5yT{Zk^p4SoXAL4cKNQI`#7ec%LL^G ziMo;o>7`%5(vhyuYCOSG@y3O3PP&YW?5~+IQpW=Pq`!Xi z-~Y$|Lq7cN;eY$j|LO@R6R{_sGvG}56psmc#tZZDX@XTXZhCyPDIf{XDd$aPbnqW% z*Z>&`u-JR>vCl%7ug#CKjv-?~DGzq|^pCjl=v@5F0nH`PE$ryyxf6%U}nU6IU@}bJ(tno|Gk}+F6WYzUz_N~LIM}VErR+u@#EgG-q z_IC{OyZZ6T^G4(Ts!fX-j%OgcCgI8hx%L>c{zOY3!AT;*l0pBE-~R4foZtT+w%$8f zuJFo3)+k3@!03?7#BZxv#6eW=%Wy+FlQ<51=PCS{6OX7(q`75KDa3*$~%vdv? zcqT3y*^;du+7d-mERsl&qFBI!MzqBO3oN$Rx4ZA{+kG{k&-Zul+l43Rm3!|wzw=>ESmikeSS zW;d92${6w(s`zQg`qBfc&{_NWy&zTQ%4x53X-ct#kaBCzt*EzdAdk_uY4HDO>o@M> zrH)I`bD-U`cVnBHeXw1)}wMLarEcKuN>L(HvhME*0^wGuV+7FZQZn+XEs##kWAkTc9fBmJwX%7{`CTo_M}UkVUp zhK?#Jw5bw)R3zo{fOK-qNP+7Jkb*>Ca8WG^5ms=r0;A%^PodldFaOeucr`S5jSFA_ zV|-BUMX?4`uf>I7dcr>!p<>un5@Ymh87Z{WUJNM4Y;M8$z=UtPy|E1u3>W!! z{L4Q))@C7m&z9HPzPq=y)$1Q@YsN3PP5XAY>6?ez$y3+b^bHm+?H;5(XiF|bJapu> za!@w4SDYhx-t};#mSAZe7>MH_93&#ap+AaEDYcZp=b63%r>@A5x18`_d>s`Wm5=pu zM!U8_DOs^4j=fGCNtoOKQ`?QXQ^OEHuogs#1T)}t*F`ImLVqF5wT@RKDay%@?QraS z_F_uj%JB?XwSK^8veRi0@h?q6wGLeM%;Zn{2}H@KzRwGo$8Tg}=ci0EBJf-~DAVaP z<)fqR(3mR|RUSOT3SBfRzX^K#5vINGNMRe~oTh_N5{=m|SB~H^D zS{#cYA2D?J6~2ZcqcI}dwx@VkWKWw0Er8@fDiQuN^O7h^dZP$2cuECNk_|q1Ag7Z9 z^m~;ud!C}*Ith9d6l~#^FaP>p%CanK*&aNUvCD%c!l4U>gn5{TcK`=jh~iXFmOnDr z&i~`~&eG4d(Jc?P7fzjO&mOzh9=qpv+U`TU+vK`?S*SeI?ml>^UB0@hUA%OwU7PM? z!q_~+bLO-eJ37$!ra?lfQX7S9s!FT(?J+$p}qqC*p*L z1Otn-pF2`bVX^lFLTm6-P7vG82uL2n7Zv5z_J9fQ^x$Ms6mpe|va47SSeqLl(C7=o zDAUM}^VBmFCG_oTe1QZ{o=AIo{3K1AZ5vQYGDTQl(0I$s;oV!bI75AG}y2d55N<<DN&;RIVe%OIHe=bE8ktY zLb4T=TFzU?EKWkh07GkyBv`=>6-AexB|p+BFIm1N4?LMJkyl0y|FpMEZ>fUkxy5?R z9a>^6GICUkST)&>lZpmuR1WjY5424a-)yTU?`>Njel+#J|MKegy`wkUWAFK`wrl^Z z2?XmV4!2DQ54KBFo7;tpvjhSsb$7@HeD&I}qRIAS81fk%K|V0DCS2s7t=VMCk7e*I zO-{QaYVlKM%5y+YALRi#R8l70DRNLIwxyoRxV`q_FqfxlSnnpZl^1oUE(?A*n$Bz6 zqAv8DAhzIy4%I5S*9L@ibmjvS9i`9;z#;y;w;w)C7S-In=&^xe^-8|IwH&6p|DYcz zPC(VkEd<%KXM`M!3VdJ+GV~Q5v;v+!aP?cPV^Y9ov|=4#hI!MF9cQKDu|M~y91>V$ zsH3S5j0r@-_(Qq!={^sx(jZdB;Y>c2E>o-Sh-PVBBBeNqI-c90CQ5tWZu3>cirR%b zKLwRH2?^F|LldlD9YAv|Bt5+e3M}3w`t|sASe4kVCT27M8Uu>%8sjOVM2#@*0XzXF zLo(^B;1HCQr`H3ZB6=B!{Ab-zT4NA8cPU+>Lu>5Ckq##zT7e6U$kdpLI--yOd0cS_ zdhyzaz=t$=Q*oRPI8g;w>KP4$QuFAX#F(~3NU!1baQln4H1d%)Ft#&}H_B$NBaaXW zm=}Ndhhyz~uiR{p-}hT>$KE~=+)E(X+b&(*(k@&e5X`VF2BN52g#5Y25TYYEPn9h^ z+b+NPa+1*-gRZ+q-ZrHmv?FI6RtXsp9nq`=Rvi@?P!RmSZ$ZcP(qvg2nRbWRCowkR z{d%0A_7jY0B5>Z_Ac@Lm*_Q-NR|q!ospE2Xd7(Zy_WBj`s8%j|Wp zm3E4?`?RGUfg_;glX&0bnspoQ;JLC>Bs@vpi$4Avc>6?o@hMzy&ytdnPH8a0rmYp3LVyBE51Iz3 zD0T=hJD1JdO#8^B+HnIBZp8e;Z@70Wvmjkj7EB74pU};mh;|ZuD6?2&lhE`U!uV%U z;FgQ!@lpxx7_M7;mJ+C&55kJCwm;dC5pR zT(eM!k|aMd7K2i+G*P2)=UVgJo6S9Zag$QdXp_mV7&I`hVLf6NpMkH(M>}bp)_Cg_ z4=Y`b14@2x#6HFLz{8JZxcuE$8QHxu)1J8RKeTOkz0p>$ySuHMxVvpSd>12vtptJ_ z?K*)VY{@I@6kgbjfvw~Q4P2&wgYg#cbKS;s8|BFF zNaU*4s1jtJ6+}+Vj4j>HS3k9wX%ThrD39Q%qpp=*I-xF@$Bsd2z|>kqZKseCEQH5a z6O!~DQW;lA%s?CW_O1)z>IHVJA4UTzK4~?Nl(M3)zf53IHY=$DwfHKmQcyh|STM*m zP_4uO6-11>H)iHzD{;gIfmIfcSZfic97)D1ird4Yc!;QtLNY1_D&cf~rn)?_6gb~A z1U~5!hYk~gC;Cu8)n3u{BN2?Y1H^-LW$9p2HVu5W8^ygA+G7VUAvO!ew<9B9xHF%oe{3ZYm2Ay#?`)ar$Q zz>txD@+3&%12J&&g{j3Cl{G=0VZB4yIDV(&zFsycsTbf3f@Li>N*?&MH_27E28HMV zXUo%vwjqoZ;OJ__|0cP}oL~70XYyQ>oqrIgg1e@Uo}*v*6TkpiI+0mz+BU;}A>x0& z9cifr)ok=aR*8qBs&rNd}g*4WOv~7aRyPvK9!NjaLB^ zeVTViTVBerYSJF1HSYs(&>|9vFy($>D!K~Itrj)cORO{fH+ElHZ48!gG{Ay@7Cwq z*|*2q_{5IJ*WhEE;!=hgI0>zen97au?)iJ~e}K`)&GyXeYuct0zuOM&`$F5ilc~Yl zd)unP&F$dchua$`?75UL)gqHJcI8mLC_HLtoveV8u-u^#MwzC~^Z>3QA@A4;wxsHX zrphpn+m23*S8uE{SwMd8NP)AFE@dv@w6I4ipdO#ZE)F1!YaPCi{zg%2=!GoVXJ z5zi>aUY?Y1ZRTlHA2nyIzC4v;;u|U|Q(M~OOj}_7m0n_J`WB%6i>**Dgu#_J z0u_92v0%itLrvBKPn{B6DDNu9S&({+IZh39*QgCimjblG1PA^AD1UKRNadFYtStkhSP@$1} z7*ZBKJNl|8<=Os@kD|x`;q;^iO*&jhNbv1g_kXo#zCPC;eqsuT+r*G)2Pn-da@aHtZ|Llu3BjygUcSP-0!_>4ceUX|=90v|Q(iY^+Fr_CQcz z&uaTsCVtBec+}<0RD63EqoyLDBtfk+8rwC104)wL`+xEoLdi%zm*xnUU7NxtulRRE z({*<9x!@>9Q+V@)I#RTRJo|0}`0?AO)E`D8xiiuFcm-Y*t2M44&I?hzzTg7o+^oj}YMo7*krvIpr4R zAGjepr?jE6)9X{WnC)<+eIsxQK0|ejY{Os6K_WjU@dcKA@uWAGhSH?=VC5v~>g#dB zOCnr(9dBd&dEa&I*bmzD-1{ifAa1rz!@trlpMSl5_qCsCH_rZXTe$fx+PXkFn}xtX zJmgD6rrYMt3vKm;yiTA5zW&N0sqew}RnpGi0A4_$zkNma2*>{1&qDkBeSO7exE-oxFkfLcuTY#x)YDIy@035hhMlIW0;>^o4_l7 zVF4lCDPwc_L0A1z`UB-reaVx5X4+iWq&69+@2(wbgyxDBv?(l(L7;V=r|OwmK{Egm zmiDD{2wvx@ONAGDrM>^DyD|2kXwSXyQk!|^J8kcdKWKaRFSSiO zj1jStzCDxt=rFtVD?BmeRd3Iw#O?c5SP7U44v~5PjwJvVz|8fkd0j9 z7-XCRh*~sLfH+IX3#!X?j9OylwIezX%|(u#16Y*t0^+ZLjhui{hmOiTiHbS5M@}r#0rrI>iZ3(v!eofmC#Sr86=s z4db-Z0<0U}e37kRYMQ=&wZq|pW!bAhQs{I_c~)mg#XdpUGE+vUzLwuP@}h-6X*a&o z&c~?BmizkZk|)nLn6#b=UWair(f zu!A947E=Ua;jqGW&$j|k4`VA!;xbAWEmpI=!3_7ka@;}!mz2e*q~8mY!yJ|3T0sm~ zOTc=(uJ03IxOujKVQXM8u1*f6_~K2xDkNpJ>9E%(R&m>E&AQd?!3}G9F2qNLTYKQ4 zhuVcx>)NZw4!098va|6!FSflqf3NM?x7;>vf1Dxw0fND1oOZGu-m{8DxJ@|kD8XPI zyy#HSBJ!R9kJ$veU+pwNE#87c$;H7q}?PYW#Jj3mgXjy3RDA=pBCC`r zo4NMBCqKYEbFMw}(*Cyi%(3>=6JKs?H{I3NuD#s0++@fz&s zcwvecK5;lUJ^_8oM0w^$@(TUVH5F;UeOzbN;mXNMrdbDoF1IJA0Wn1e0CUt>eW^-p zN%}35neNX+!1-78yvE`*7V0K&V4h{k0GIU^Y%YZ+#g0AtNBydvV>x%(s%;1&-8JGW zSmFC*Fpnt-LuT!iO%w5rMLLt4yp?+y9~^diLlSOfRB9vk=6wOjbWCZZ8d@T%dh0QA zqg)=+mM7XheAd*d;2mWLKSy;q9A|}UZ{zf@{W6-!h<_Cf`eRpFq!O0~I7aa{h~n{R z5tktB(fljQGJN7fad;kb4doq^M(@72UQu+|FF{hF=ux0VJvRU)XwXF&!M(;7fyw8Zq7c?o`3#Gd-Hi-FZk{6x4pZ6ukGHq+}3aTU>je35Qn+mCYWX% zx@(v_BAZ!29B*eYtcDNQ3zXJi+rRxw?ai~l(#~K0!}gwm&t;{5h;;@Ek)`r9ba+{T zGV-%b<|wc%!4Z5(WVpe`t5xhHc;J!uv4HwO``*jS-Y_aOy>BfwSz+au)#Y^)njk=qTnzr2R=#*8F8uSI>e@emWHgb2oMo4|36-ki`=S%_YIlJMR%upyy2#{_T@WKq|wF9ak;$%8( zH{5eL)o-o|7QM$n0Fe;_NEJlSs}5TmE_x3v!%gdORDxE46&ZV{4si%a=azPDB#sPO z$~kc5(d5&p?Q*NEUVBvwQejd7%c#l7$1kWj*14)nG3`iU@g$7-6(po_fB-NuJj(|| zsz!|v!~$V)Ku)Shtsv z!3@D*v>n{dg4~k9U>&>oXmy#burN5ec1x)7-}tKMaK0l9<<}Mcv{`IHSFy3;aI$6S z)z-^6?QJzCGW-vd&fDMn;6qF`?rneZg9B~d@^7?<9(l8^B@nDx!>+=+_P3i0``c@8 z4l;URL`Cm2=xC)>gmD`DT`Qt72FEVAdS9v(h$K(f9CI<- zhv`Q1FChQ`AOJ~3K~(4i;|$SdNY+(}@-#fTu|4$xf&m6}Yxb%3-1F~m-+SXq+cEKK z+r8`GwO#vKTfgb!ZDf2W!@}t{$=1)wUB0*Py=~{xJ8f$745Nk*B9Li_xtk>kMgg6- z>kqb7^5F#f!yg4jD@V?Cu398@lp~)I&1lNtV9z$n8pshw1kZf$So`dst!|r#KGS~W zC(g7r>tAmZYwu|X_q?YaJ-&sv?O~PUy;xLE(_S=@#eW@u2=tam`fOHCCNCB^SXJHvUEmbS@E6&F7d z98K4QST;)#w{74Ab-`F%zG{r-31ZZRm$a%Qb!maQ{v3NTq;-`axS@{nD9Q>bod@*O zVmh73kMnoIAmbXfpZw zgE%~^-tHo++cIR4q)i->mOww)(s}x`HLg%=G+)<8W>7` z#ck2AW%bUXk=_=T#E_uX*F@i9%d)Qn7bGb(vW{`%Mmu`+Xj?V&TkWpH_qWI1zq-AC z>XVFm2qYmL=LEKl45xPfFWM^X7}qXhIq9jafXSvAbN#8iHsv6Z5v2wWlX6t1aXJk> z5u1W)DBzx&;_Mx)d3WkH00uzusw4gk7(IfxFm2hM_uFd=uQ)|a9rDHJklGPNKT0gT zAQGpb99(a*w^sJzlG|v<9zLM74O|WT^%MS(k%h{mcUtW^b|hj_T2Rj&9YI#PX)7IJ zfp)OvhouhNBq*`jXxJVtO^K27Y%s7Qkcnt!d)-+?&b>63Je|7_8V=9J(bb?YbO#*Q zQF=(`v=|N^|E1mhqirG^f~nwva9z;zE(IbRU?a&Y7RuZc-7ARN)fCJqMEU5QF8D<* zY7-eTAZU@qenmP4%C!O!)PS>sUnIdxP7GQD)KpcF(B_ZxSnZ8=YvR}1$kZ>hkoGUz zfVbx)$iyKEr=Gzhrc7QR*a8O{a<~oL_)V%$wq4t=w^L^~giaTnVz;!Fm&|5i z4P+}&2MeS}fLmNq_ev+dcwiXL+kr=DQdeu7}&s6edptOIJ?%%ADy z?d{cfbP^i*mn0p~qaG>OS>h1nuHw~G17m-mqR%j#bvUgvBIDjMI(-)&)h{kvRzYgDUU8_0Ey7j%ldK^=+Lgp4=`aiWliva6F`MpI&HxJ z9Z#NEPe%<4D=>vCvVk7?zMXnJktW%|c6fVQg{@K=0%y7_+K>?QyeiLRsxyx3#>5~ z+o7&P^N)V5Ur=1?Xz9KcOqsX=Lg^U=aG9YLd2un6VGkrfd9;=Ka??*0Y(Tnf29*Sr z782(uGNOFTz4UhD%dZ`7yLbE+JKlEiGRkkXgZKWgZRX7Hw{vfNvu!{8qu@|I)q7AS zNt(o8SZv?u(gj0#3L6+Y4z+j7XTe*as>(=ut=h`px;D9eh&u@zFtX8h?!s7`ndMr?=l;DqlQu=xC+DFxnp0Lf z8Rh;6!*#!*rB%yXj0?hZ zt8yK=SQnc4XAMVF@!;5Q(djx>QV;~Xdi!X)I*m91^~*-62W*eGx;}8L6JkceY9%(M zeaI_xga+sRI_`a86qO!+YKoEP6)j$Y!#ddS>W&|L8@y@ z8fs{#_;md=#Zb-l2$1kQw{rW&Qd1Eb`o)b=6%G6+Xy~NAkV5&})3u2?MkUaF^HQp& zjp`rTz?zS?>9_uAyS4KHR_iwxeF9inLk=+1(?r@!5%Pm>OpL280p~{@&RzR^Wvpg{ zV*p1Pg@nKMGkZZW-(EQSQ|;LCBklRGPqj@WZ@0a>*lu zw+vdtwkOo7kr@|VQ@}VhrA>luHd8TDGvz<|S$B%w4CL4wZK{uu)wLv9aU<;ry*q8n zQ-a7Zc1N}d`$at4^F=n=iuGb6+KYVIhDUqJBtNK9K}|F!r+?{%phZsd7UwA7@vQ=b&%uE>-P*K<+$NQWm zt=O&D)NA>x?b}e%!bQMK4(tP{#%y8*5$7420$5S23^(-jk3_TGz<<)w~j}qQBFH5PtFi;8kLU#s>;bhyec;QR~LA<^#No(vhKD#@gqs@q z`8Is?3%rEmr=l#4*hRYW0BN2iE-s#!}T9-BLyLnsPsaNoOT~ zS~HlH##fN@b~|JZ{@5EZgY=S_h6IFb+m1t#0Z-9Yu#qnV$|O@NKNiCqdC^k^PdUVz zm^OrEXo^0$h`psw8VP=BF1QXJ+-`7-kP2+&e=iUD5m$$WLw|nf8QI|OiO(aZ7AuOo;uX(*PqcIwC5zl#iRx=Zxp$Rk* zJVH-z3v6)PmMu$$iIW~*aZ+&g3;@Lj5MX=umr$C#pqgZQX|gR*TGEeHR7!+M0cIW? zmMhodlT~r@O-54xOxS*M9o{Z3xsAei##`X3JowT7{=M7V`ImmP&CPzGt>VRvLm0!t zfG3>44-0K%EAE@;|2uFpPe*ayYH}3gD5-dnpL!lX zGJwZbJbd%`6HkJJ7m&=pr~UAipKjuN7r`cn`a|m1ibvv-{M*HE*=mkU? zeJ=-^ucQ#BN~XKu@jsBSiIRG>{Aw+1q6DrE4WA4cp@%f66f01}0x(GSg9;2|-1d23?IKdNmN) z2yo))a3^#KVl~&3;KSr~pGJ7f(ub#Lh;sa;r;B=0Sb7mtu-%bYWil8Z(}?RJp44KV z7(`bVQY~gG#A$z&{@jOBlxn;A+O*KIHQG>^nJNTDoR4PeZV93kraFGvQ=t`dXwjdtzsBB*)rzXRlC=__LjRe43{2BbH8{Vmp zF{BpnBj{7$q?v+MV{6*Q!B4iqS3cdg-T#kS#`evvzSDq!Mvxd?HQ4s;nP>Ri^1{wN z+i$QdY@RuKE%=-{ztq-^{(d_<_si6B2O#e}uqmR?5Vt?uK)uJ2F+jU9eVrjb`ycpo zwBRSOe5!nWVhy_w**^fSbF+`O=U+V9j$RyWyVm?h`@m1`Y41I>roH<1gORr{o}i)h zxNs0!W0~@!&Xi>o!$CPX6b-{myi{b8BI}e~IibucrM6IT8D0Zl&rxCE)&)HG)qxd# z)@RI)J}Knfp|-)fP!sO@-cvEHG>7v+Pj7qPYo`YIw8tRi$|ltfbb?<8s$gcz*g?me za4{q5+PiBZ8M(-wsldbjIKBP|&yWb?9)IzRGe8{7_uONR47wqax#BSif~2e)nf3V; z+;;q~lM7*Dw4Dw|mnRle?nu}Jk+j{S!NGISuT5&-uJI56fMaT_0z!$}EzDuG^8CNj zJI4vQPNbp0LSS88^~0J`me~rKxrvRr;E)(YzOeRMRk1Q|M>COGkN-woE)7l*gdaZV zI?AAJ4NDS>OkpTqQKEpQXeHengRQx(xfi}larK(UK;*Ib>=(MEz5*VCH2TwsdG#Ns zx^WS?c$>567+?fY=WvL_-v7XT?S<$6W?OsmpR~0H{&!yCyxi{EtwZ=GoZ+@+a<;8o z!yOLaoQYf)+1j~wHERnP^R~_8Wlm~~_6%hNwm!lm$)=*jMW?lKQm>O7g?OR>VN2s0 z8=q_+_~7<9@6fut36Y3^bHUTTufI zeNvJ94x_s`AIvhM1y>;pKfRq=CpZlP-Dsf4!?D-1hN@jyjjdStHuH*%Bo1A{K1w!z{$6w}mHD z6d`xLhF}N@%bz~-LdIXj~(WjTNEfF9nRlylDppIYN^;do9l}6l27+aD|?nM}y~2F~GS5 z`c3Wrecxnmx~aW#oCkE6K0r-7c;EX7Fuy?yt)034k04D#B**ejis4&*bu09Fq7rfg z)7tsd7g;;lO#4+Ub5w~ob=F!&+2JNH(dK_S`X0rZkfU>pDJ&IVx6O!avp})(ffo(WL|(S>f`9#bUdm~Z>x#lCAlZ8$EX{^T z`3_6+RJ26K!jP=rcUaMxu-H&QJEZ#5O0c5sto2c5e#uch5&}=Q>oH7Jd5Xvh4!|6R zISqp#a!riX+4z-iD|4)5K`;dgOdK_So!v#3Io>a~$clY68VqUJe~u!OydN9_RR}o{ zZv719X;(Liy-J-53oZQPx_+#~9|Dve^OcGWyKo=Xr@S0mT2zi;9kS4uh-?b$6!!$#0Cl0k@o}_+w>>&6}JX<4KJ3V z2b|b+V8>T^!tleP*P*>ZPyRB@j{@R4RVN#=G)h{rt0moa=wALmoqG5Mft&o!ak98% zn;^^()KkuT;|-0_X^(3wel#|E1IDA_%_|m zNM6fCSp^Xl-VF(>& zate$%R9HQ(Y*SvDs6?5Sms$CrzVsP}&^m%IZ8WqE)5DLodv^a0vSXEgWT{=de5(D( z`);(i-X3i49Q)^O`QyTRBS~F)tx-m+#YmeS`EQ`(Px)hiI=WZp zW&FYwoBU6n!(rDEitj?ukF5HU*sYrvunpEAY!nd1WHO}Me%TKhoi02`k4MeN_cHjJ zE0eH&CsTMt27mDA(V9iGr#@d`gD%L@7C3yjLF}yvcSByBZrE$;Mn0T{Cy#V)8P#!~ za_z_VxN`UDYNl8cnwI&8Wpt2r3@tjCxxjh5xaYqzi!W>T!H$#g-ovFc3ReG>4I0Yj zBgqkGm%5w;)r=}^lZJZG9gZ`VoY{T-NOs>++|AcBdIq3eqtU zEm?<38c6B9Xp^tB9?o^pE?Gc*ehY8-BMZeTbR{Kwnt=)f%ECfM9R<3~;@<)<@VIfE zh0f*av?IP7Gv^rkKiaNedA6KSbR z?{@dYKTD5(vrSK(;tGyg+1P?Ry~QOk2PBpMZN6x(>?-JU}1A zquV#SG`MgM*>cet0l7#v|E+WOZ(j8o*XKAhh{jg2=J(Mz5T@ z@#ecPezZOIz?V~Q-QIy?c+_#&X}-U?fbi-0evaK;PK1=NiB&)=j)MXrQoq-aK6v z5>)<+eZWIaRmpIA_^t$f(rMeIqO?2jeTd8G&}@AN__~!zo1&yrVr+m{s>3Eh@u_0jV=khko#_I3wpO zRApE5b?joJHBvFXGsq=HfeY(J+VDdBT*ukH4sBIl3shwC8w0SM%C&xx)8NrSEiyJr zXs#qUj$CEpGJ=XeLJY3uc4*g0=Cj+;B_BCC{ZHEJH5b}lckgR&UHG3durfT~Mm#&i zOEA#4^`m%uWX>1oFO%Ut?E44;L+!%J-)&be&$Ua8_P5^RK0I}J?Hex%ApqEs!|Q5p z`JFxSnYMYyC)2Sn%)i2P;2^vDrg(PzJn-jO{Np=t=;JO2hf&vP<$bet6f{+Z(5U3?r6&nNc(Fp}t~tRZ{lf^O=Y@|gBIqqU_36_5Nx?^a0Tf~pfOQhT-c z6{+P?k(1!7_O(u(`Sv`%S{J@xZ!y)KnYqq`RWogXJG}PJ4k%NTwqoPhjiccNFiIj@ zs;3N?+cPxe>ra#?IMRV2yDDMMojvay;8FUvp_)mU?mp=k*SKQ<6*%!thK2r{w zHQ!W0wRm-EmS+L7Kn8@mKr|&c=YN49>V;5|k4#c=T8Yz1Z?QleB;0l@G z(SuQpu|bD9yyM@tm!A0n@9~>&SEpM0&YL_0!#dx(-9N`px(jXj#;?-mI(qCGhWkjQ z7E6$c&hNt5x;As|X$)+fc|H&DY`UMd6Lt*J%Y(MvwT;_z8xOQU{pzK5-_EOyy1+k) zSPX;%3pn@pxPYg{YXpV&p^Tkva{YZwH@@4hURi1zc&X^fD8pRvnN+Y^DBXYq1!`6? z4p>u>F8!GNub!1V1h<)?U#9d=xo{N2K{@8gDbA1)6=KtQnmhdEckHTN}>VbbXs|| zuC{NhrrR=7#!_RR<$bC=+qFhuz{Avt0)Bn4$AZkMIC>tp%83njIYC;2R)GQZU+|Zr zbS8h21s;LYZ0VJ2Yj(tA6fp#GXdv8f$`gvv)6e^2Dq3g9SWtqo$C0A~0VZUoN!sk@ z!qU;--jnfCki^`k(2>b2Z~xcr!r7PF^WW#eBW{haUpLtH-8a$x=>L5>ihuNe=FIdA zTX(K!KmI!Ye3)jcurNNzt@*Zb>xH&%0ywzsr_d^>Jx)4d9BO-<6aRS*Me^o2MwFJ%egeH5w3Yd>Igj{PTQDYJs{%z75h+M{urW!o)!szm8?n=A6lg zzsktDA0aKzGm`|Iup&q>#jX{B4;Yyu!!8IUaO6DD;mN87EhSJpB073fo5I)$Eh0zL z(BJ&jr#|hDDc{>Jnm!3oy#VpJ11_-w1Cv&dpt)4>RkiAee)Bk4ak%n;OsW6?AOJ~3 zK~xkK0f|186PHk*Y zJhr9n+cwqCA7ey-AWxm%*6w}ulkMi!FQXWuf_nUutads5HOZuo;;kt z#TH=#<=nMb_;vv!1=laM5@ofbH_m%Uh%`zv0a`yT)H4%A5*fz z)eodN4<=tZSr&@~r-5EIzw+w3wrkt*c75*sY2T5ft?3K>@Lah3nU@ZtFSoZX9Z06_ z#y)Ia8VD%z0noen$?Yx5W1~_PI}ro61OCy`rM7eHT-&+oows@T7 z?-ozX&N09Ag|GQc86z`gn7_0~;0v(?F*?ZMESgksX`>1pMr!ekla)lSPGTVtGVSIc z|7ZoSEHUTT!UuG}URfMIh4BxqW*Zl`Pzm(e2NzSu>^AXeE#OJJn0A4&?A z`^VKdSK#U@*Zc>1q=Uaf>ZuUvHyDr;c=0(Gy?F9PdS{|79RPA`g9d^PpKYT%|0j0p z?PzD;e4#C`;r{p0&$j1|KG-%4owWJl0-QH^G3GUvR90W%Z^H)29!2>W?!vd|$uF_$ zKhPe#|2)s~Z)t!2EIrSNeL`#ZJT%lMM!(O~g?m^qe20DeIDFd;zL$9p|G_r6YHg+o zgF|cD^wn>{zgvRGFp8wDgOUjE|zj#dc`N zpR_m5{uRnFScC&BF-{JZI)h(vegQ~`EUHP+p^Za3zR-@H`FI4<-#NhYaxK;vw{@aY zX@MHNpgI`NOK}-(NM{l>tLsb+Wkg-+9MIW+C`33yR({+8w{_D3PJflxyI)0y$FgH@ z7QGG*aT|7k5!K9dZH$MuMuw-b2js@q>$%y>ypvI#NrrkMpAi?I?;sEiw?`kGZQp-^ z$E^8q3MLdg)Pf?~tYy(Yw8~qYS{+#kgf^SVVZ|;vQmQ~#8FlB1b8!eyxhY?#0+Ab@ z(imK-(|0F@TEzx0>5>owN4|Zz0$*^xyTQ+bUB_7KVIVab+A10=Om%eIQ9;O@%6qHk ziH2EJcZ6Q9O}RS&@vg@9DJzyTYeWDtyYDzgR=`H*o#L&NsBb>!9UD=8j7Ku^gJ7I@ z_X+q2J@|`mjVis5NP1+Q0?f*bB`E0Wn!PdIrmrziWKO=8-rtv-@D2=y^t?X%%-7ra zzQ5HDJoKaO_3xZ-`wkpy?|<}sd;Ntm=w+^86Aa%+$Ch~KTWeR&khdeN$5FWUvhQ+eN_&Yp z=u{|kWB+YC`_8{_dk*Y^et6}dcBUyrWZ=2o8I)n=%}7<+LAADH&wkFWrDW?+n^<}~ z>M}W$(U%r&%hapK#y#&bNmCh!^{^j$;Chs`YbVfUOM`PiXdBj@VL!zcMgS9Sboc?N zq368$R=YXC2UDm27zcj@yLM6aY@4~x`wiE;hxH@gyU2%AZ{0XSV|!R**+igV-kzP0 z-Uc6`=XcZ5czfi?5L>DjvTgt{>{e&uRKwk?+LAO^FCKEahVB?UNBqJ-X5d0o$qpV- zDQo4=c>+ma;4wleo`tBu(gBd|8j^uLI6{LJDphdpuQ*Bz$&rM2HnIpP13dwLgD(*a zVv3o-cTpNzl36sV#-bP^dgdk2;ZIkP%^NsV*KUx7CbuuCWP#k@dZ{xzrSDt1#be>l=vnDCwavRe#4v45yXUdL(XLpcob zfiFapqrCRLjYGlo+>sfdD!YCSS?z#@8jUP72aXKE6=zre%E%YGs;s04nKJ_fhL^P# zUmNz8YA7YtNrH~GysYX_#Yske1#OM!B=qJA9Z5vjF<3XwK|Q;cew5O23zPiLe}hrm zJ50Mq+VuS6ZSVLwDvh*PU%td$m=~Fn9clY^{sG@b{1`I62urKD<@z0DHbRg%j{er+ z6l}&~iZ#mHnMX$1^~W2Zas9|ut69a{mmWGY(q4So&v)r`u;!URW7 z;3Jn2YNrqL^*Q;EU<%%#kY9@R1B0GZ%XEi9BJ);V;k@dT?SgQhY&ZJ$s9#n!_#ya6 z2lR?_iP1tfY$^Cq4RHEyYV?&vCZRGbtx4jLQGUV%FnMUGt7V-qPpQTh-uah%q9UL^ zCJHf}Q$ENB8c!uPuPMq$67epQy?Ey6p@R&LsmRf&=ieYks3AHhRS*mIh?&QUL9t4N z)!+kyTyV$UjZ0LeT-qXdeCyc{wMXCc+jP?CLHUZtRy9_DtA<)v z&vl*AMH(cdPhRtv?9R{)?6DPKVprM#06zHR*OGT_ey&|S#bZ-5C))z+Cfj%IWi96q z*r;`|9oWx8-=j~kkT=k-p86UjPa#{aoIM#lR25kbuZM>lZD^jdxk&=T8AfwEcxmfF z0)gLqK)9nW@_<|LzOJ9(rZr-j4a~-LZ&vo_EH4Sql)3HE_9gq;W zQ=F8`OnZ{8%%cefSMnDD7d|Dbt_qAJ`O%#j<_At$!829VwMjYJ$qpy70@&XdH|kbT zV)W=7TwJN9AyoA78a*fVJ*y2AEeb>?C6ZLMcB_)&kT8CG!3D&OYUg&VWJwP{wV^`8 zKwvh)ug<7pd0lN;bGdEb%+qz=*+FSK^YFmccKyaE*Es0>Xghn(pu+GD+=uqVCmV2< zXLhzP{XT=hJ*`bKhxc*!$$d-%Hm)TQ47I!OUfa&RwYD9;?^?UaC$m=Z&GxDD$1#%i z7{emp*?WashYRhsH(EP>>|5=D2U@%DJ{h~!rY^k9MxM2~eRq9ofR}vouKi8Qfua8D z?O7>)__6W!;QQ`DpD~;Yue&*C1G0$;v?Io&KC= zC#PA!BoF{%Z+_+cJ8j9G!8Fi6EY`0%lpkBkKj_H+s#o|mYUcUocTQiXQ&g(cGB#)f zMQjU$zJBK0ZTo>QBAab(19vlCd+8w_q&kK^r_j-Zpn9Y2yXPlZPkp7$UjC2N-AX{` z<*755B9FnrX*vw(;k%8)BRpZr8o|I4-{xELb8YXSo5PX!P(Gonqw!9MK^9>dDYzT@ zzIzwjvoCUskAPw@mD=8Ui^Z~68H0N#Wpx-#e$}q0M%Yx%WUS(Lhd7fJc$&>?=ciGR>=NjH`2AY4AKW zbmc1uI@Dw*3#b*w)%FULDTguI5Kjz2Tr?=V8d&gma6X2-cz3|6KX}1Ic z#0m+XYgl6R4@Y2ZxNF;uwr$f43qUhGXFr3n-=IBi<;3w_d@yqB{4827(Vg=2-2mHn zty_0y8M1P=b;l+g?{yrLVfPeeIN;h1OYPnxd*I_R25^v}_$hFI4II7<6K7v@grVaw zyY|>O!29+tot9p=UEfnr~nE^F6Fe^M##>TkWl5yif1!HKrM_ zw3BaKqbFz7asD!M`fWUA$*sgwJX#H_ZbsU~NI}`zQ}Geuc9Y<=f!k6eqXZ`I4h-?` zzw1|cF~#}^P;fWY16~5vrHbxFm~&((?IO*sRxU=CegK^lZOewY+Lf8RDK}`=&{AFJ z&jk^ZCSmnHd9DFpJoP5S?Gx?#{D;zBwM*NSNr6VaLQh9iSp^7eLp&TYkxqA9Tg0Df zhza)6jgxI=>a)DeWSqJE#dgm&hu$AHXmC{HM+7Z{?U7heZ>)&)X4f zUV0AcgUJlz&Ep!ekl7_p&CBqRC}N2Wm^=p3>pT96F#tL=AsF2K+%@@JdtAliOS`t; z;v*S1+uD(@waIZ7bQ)jgSv=4E@v}II9)m)L8PcM_5R20{p4#JIwP8C36XvUKy7AAq z+Uz;>NZYdOUOu_?-S##^dG82p+PZ*IonT-7(@Z5!R{dohZ+Hy>;88{c=a~j9@P*); z?AEKd_dM~jU&?TPap8HCI?dnBcJRG_y=~j_0xQthq@ulkbURan=}aGPF)A1xrRN*o z+Fsx;!-Ag~gFKc-*igjdMjU+&fWg3pxwRX40SNQf5xx(wisQ{$HoL4kjJ}xfAPsd{ zPOQPI9exaWG@L0MhFtnAUC#|1X*Z|7+19R~Y}anQr{L2|^~3x!+&D8x)MIayDZ#lf zo_)I=*!OmuUidI1sH=EFo!hW7uYWnXl{sL7=C8B6I9c%^t>jVGjtS_qZ!-$G#(K$z z=sbuf+#g1q;bVmFI<6V{e7kh@zUWoCM<(zbA6StWWTDi)*VegRztSUm{!^EWaP3i9 z$UQjX6F}RW_r7OY&Lsu-l|SVM{Yq}0HViC2G3ai@;4>J5)2~Sned<%c@aeuG;FAP0 z^~We%9a71ZgQuSGTdS2-X4I*(?gS(~VdSK&$4fyGD#4B%_>twn_eX#ekT@RN@pRk5 zR?e|ucAYJ}%u6|l_A{>&@!vqO0|Wqi_yM-+vRiK%$6Hu9!^-+;oW;+5JKO17?kR*? zG&wtSf%=TK2@)f`Li){PgG?FTVVB*{wu|R>;rxyYF4MD5@==UWv1oUTZ}hFkdEWuA z^T$=~_-kueVScEsTmJ^%_InN<7)^}u+3wNz!ZXv2)N$_*>%MKRx~>>0f4%QJHd^k6RBeOs?aOgwMDb1e|+LFr674-%8#dM)154WbkUc zdHE}CY3y;@wKr0Rpz8iOQ}j`~oW~F`2zzA>QDdIF`i!Y4kDfYa65J8 z{O9QD*?2W{r7aTl_H6xZJ8TwqBmX8tu2==7y`##!U`tIA@!s9k90rcR# z4|Lykr%hmKo<0paOc}8gW9Hvv?HEvnjC)*LxGVf^^ zS;gXOBj7rsqyw3r`bK-}>_^#zQ`0X<1zFet#vmxqFw^ti@vBc6ocnJu2y2m}vU2cp zZguHC8s(_O^|eKA?pxocD#(<*q+MV_0{T;axd2WQJQv7-A+Y-u+IBDn)jIlYS?#Y7y`*8rE=!%x1iJ^#IRZ2A2tJm|q*-Qq<|N4_0;RroV;WdgE+LquMmICr;w@W(A$cgZCFQS zAk`4lxQ7lWSUUWr$FxDI$1Flwl|N}v2h!*~)%I`0R#3>(nG%m(s=t{FUuJQ3p&dWY zt5Vi2w|$3R;=>yRuA8qjs#?_!?tMRZWIoYeeCC_&fd^mZFJ4%vv70f zICO6U*M^LSZ_b`%&1ru-xbH%H{-vGpQ_iXYhCq40T-~^%4^&cqp5O5e0Tiz9VRg}i ztO~Jmr!4u(i4#&5Uy-@;4_5xwd9DJZDh!|cj$4fDoro!_@=jO28B2X+C!njBMi(_u z$F4=HBocw&9Keb~4=T>+Z3(|M&&x>~FAB}K3jb(05sM7bm_jz!Ir2ZaaQvQQ;rijt z&b41+IQIg>w^@w+`+%?@g-o@@C7jI^0a~5>!ND^yhodk&9$MgD`oj0y$n09~+uy@{ z&Rae%iqM^cq&5UG{C4q4qXvPWZ+qX9_wm`SOKi^hQ{H=bscoPa9;P6-Q4q#yCi0uM z(PeNx!h=DJ3y0eMk3ZJdto|Z2@s?axhA*AE$WFb}>47HJ9Ac!giU)qa)Go|^vW;wD zOX}ipwyAS0@x!c72D-@S0%V^qo~2FJ2eNDqX&289w!03xy69+y&yk(`{RmRW9v<7s zYs%RI3jNAL*|`a+H@(8CR81O5->ZNEzK2Htc02LLueJSm?_kfu3<1(I=4aJh%ORqn$qa5vDQ7hBR~W*H{xEfD%O4PAt$<5Av#X z-nF>tAXBdw+W8aTXxsMxG9!WA?Iv5Q7YQzml%WT9s6T{0zVPMN{_0P0!6=W_Sw*c1^o4O3Ke->hR)wV z#%re8!gyBiNH?b#5zewOXNw(Wl6#b)TqOeN;$$LXh7?Pkj>0|NFgVCfK%x5^L{-0x91 ziZI*DDAFEY8q0op&{*3Zeu2u>ACqJQ9o6g5rGd3b+nhxml)Yy%R=4tYd6}V zL(7e~a|M2J;WBdJ!Kl?w5*+Cuu5F>hCk(zk;Cf3}DhjcFk*`I*+8; zh@DcD#kulzb$w)X8l@YpkKFR32mliMI>qZ{c9R`m94c z!e)a*4}YQ^e`iZOu2_Eo4GX8cI*NGFXqWo)(wDxK5ziYBPcz* zo8WS#?LKfY`nYiRd^>p8MxJcL^>Nb6S0~%#(C>6ee8R16(+v{X7Dhgmk;Yj2cii=O z8-7nQ6#fa;HTJi2Z%=}ApzYm$7DHZwjah=;q0qme?1zBZq^X5|9 zzvoM|Gufu6rwA6(O3zQt1SN!}!^ls1edX5SQ8rEk3`*I&#kIxeYY*!8((Av@jzwN| zj!f^x`6t<;&D+-5!pp-)ygPC3){E`USH9hjy}gswe!d}z4Bdk25}LFSMi2K1V>sR`|TySHF0XML(>%+@gK%^R4~d&p;12+W^x> z0;AN)2a^S8=8v_VJW~*7#YYrMd`7R4n=a}6kxNM?VhEhzx8VjDX?mrDDgb3~o65+3 zN>KL{c+}s7m8#rD;7X~#t6k~@Ch>e#vwBw zt;GO}AUDeZmN~PFYU7O1nHDoVLV0rO)056{W_Tt>a`gZ7{ ze}q9Y;=%cyek?EkXXv}c^x{9T`Q?D5wr$(6&Vd(LtmM0Z^!}5>3?GoCcDOk2eeFr! zGV~F6y2*X;Kcd&-112tdrk_~dMh5@9F<@bRVJ|)YXgl`e=i1ceOZ2*Y&U)j=+SPO4 z=FZ4m+qm=Lwr1?BJRo(m?b`bfxclL4iHNlQ-mmX!d-oBj*I{^F5nC+%CTT z=WTT98{ogdocts;AbjXY*xUaHyj}4f^g!T6M{KcRVf2rTH2=#003ZNKL_t*Ak%xY+ zedF`b^Bu)0=$U4L^(3POb;~mw=f2vGe&p za2kG7-gSz-Gh0<3;ve_{=4f6}dW{3w`Sr>C+K%ilPY6`;K=&>9GF5FV{z!?Pz9W!% zoQNp^3r749(-M_MdZ+UnNknAjKAD#V2r?n3O9fC1cVy`)q|~Rea`0qD(+>Me<>rAO zVzO!qE&I29mu<1%Z5L1T^qqK$=Q3!T(a~oZnmKHquGo+TH1^{J#;5u0Nyl(77Jj*H zR(06C2R+AundVJB|A|c={}tz3>5RAuEdOB=Zk1LztLVxMza6-X4|SaQANeZSKV)%G zNAzB?bs6sSRNeJko7?C9&6l8Qo|kR97|BC2+X+$}PlJAG`B~NsIDZG)z|PyHnJUym zp8@_10b$=x>2s(*(w_SGPcVnQ$ZNy?5K;(Ii^_=GjFz)aQ3sx5)o_IG0K}2qxZv7= z382MA_Y!QNmwb$j;WmARz4G1)b$H6((Nlk?)2SGl(lE90F;YJfzO?jq8=R!u) z@sAP6J_rj-ESSCmz28TdzlY3lc-YHZmOvZd;W27QCxfdGwzrRct!>-;R2v=q7JML) zk-OUPIA8C*#d-j1EnCpl-1Vp1gHPPuzWHUg1pCIME=iV;rnqTnkj`js_BA@6r?@@4 zh~2$}td%Q|A8&cJ9X;_#WOAp!h$ap#a~6QGhW^EG-%gGeBd;hLBL*U;vBW+e{1?cx zfR8^_M@)rx`4*DX|2jNY^{&%=ro`P=V5RQ;NrzyL1{S?9+9d;nPArBvt%ks5FmmY6 z%`!=7B9Bt@`i{oyd4a0RR4FJ56*yfi5*dFdj=f2b^xYVoiqt`}W`X^w2p3y?XvbZ9 z-WrR%s%n&L=WX0@#BiBm^<{d6op+TWjmGTp%f;n$U!>Q&hu-n$Fre4T2e|;bm8O$( zmw_Dl9~)%@(ZCjl^&6;jG23!|Cf_u|nDfH0zGWVCcreoMj=P0Vz=%gG4*jXHZ zt@ak!+qmB8sKjZCqk%2(B98{QRof1*pPf&PFJ6p%moU1U3`?D#e&?I)J7D9{!Tmh$ zx{htlyqo{};@_om!gK=P1WzUI+w}^H9;3(q0XP^YMTbM}r8hqXtaI}kAw)~my|DbN zZEWaQp$TKz@{ikU1V6dGjHp`6+X{hY)MnY}#1jOAN9iceVn3@`?LX6Ad48BT|6E5OjJkGCvjzBHw@arVY=`gN z*G`?d-tKwulWqI{?a<6fW%kc$j}>tQ>nPH%9edYi1D(b>Mq}EMUBTsZ#iM*03kTUn zI?aZp#We?@lhwZvPN7Hko-8kE%UqHiA?oZd%xOm}>oPJFplG0gxpq{Pb-B)=3rPoS zj3=do?&^DYAdg;`DN<^~z+0Xx+J=UX9B6U6UH)wzW?9KjPkOp~$xLJkF2&F#2=l5( z*k>Ezil1t@&(A6^1ej}`JL}y!g-8j7=1om4wXs3MJt@JAeCxYEpO?NyS)1v6q}A0| zBf8=jx#td#{Bth7mcL5`fHz)gES3pG3W>Z_gAEE`rGhc9vai?|n!_>`u$bWxgGL&tg0Mrf;B;i2Q~=Wwo9?{3k#? zJyZAej~^w^06|*2jzEUNtpmn&fW@)h?V%5{t8ei&f}J=|GaP1j;5%2^$ur!&;FHeV z=&ded;5XOt%Bt!?4u)s))Syp3nW!8(_ci|)5S!2SUd=$XvF><$9 zIYbuF!AQzixtZeRY)CUmund>ms-YDl?T7Z(^GlC~pw~s2qbO^XfQ;#cYmY?d?CJnK zX*NzK3R>aM5we)nV=(^zz!Gk&E^t1I>|3UtPWq)P7I$St)DoH|fT}1&CaKG-i_V0) z3rnf%=mCmB0SO(h7*K~ZBS#6e+(esI&bd`NtlS-N$5neejxV_6K@JW95KY*#ud}x# zAA6PTN*$Rt&pH?z98e(kGspE-R^L^O_k_h^68FH+NuuLgR(eM;gnKg9ks4R^Z{A?- zVc|TuybZ>fp0$C8?s=yD@Wv)?BfS{`F0s*Qad8vI&E_8F_=D^Ud*{qDTVgRJXtw>$ z7~wjceFM&agFl|;1((D4x|6^`R>WRNwDuD}wY5&Ds z1?bXGdK?QPU~NI&C?w87P}4z!05Dv;&}6CR=)L6GfgSDrANoaJX?6@J`W7-c9{K(7 z_@}Z$&0V*)5xDyanDe)$Qgd$RRq7MP=-ridju`2xv3f zud}`|)pqP#+n)N#pG+G|j3A$V0lppXNq16z-W~gF8mVakK+y&0@yA+=3gfA5kglX0 z&`Ns9!Y=HJkKqXaWzfib2xO9^Uj57a0OGSq_x~g8&3`sauKT|G?t8D(J>5M$D~H1w z&5#sVZML;iCbZay87ht-*$86TS%uhH{(~6#DMo+*frG>WY{ix!1d<@Z*1}jcMT!&` zk>Vl_haAq@-P1F@zWd(!e7>jZd7d6gPIbS}y|eQ*abw|~JjPfSMGas%D z86gzhj2uW}@|)HLn_J`hV?YE-g9FaRQ0D0kAuXZ@Vs72N`;Rfi;dX;Q@q|ZIesDTWgq5gB{V7NEd zhLcZ_$XOCun%5*ssc|9&ECYhxBmobw(q0);T|-zI#V>5FO^>8}7i<$FRTm9nMd<%>4>U;2Gs) zhuR7%Kw<6eZ)3Hr3?KfXcMm&vuMf}h7S-~~H^__d%Ru0?zlXq5r&h|1EA%W^| z_uYR)y{%F!A)lJbM6Tll42%FNT9N5RGSsL9k7m@{+WDKq{M7%N_9~!`<}pmT3fH)T z5>WLZ(*2LbcsZ= zEqW`NZTr&6zl{g}4-lF!r+fs1>hIn@7|xzzF1aFJYz|ucFmD9%YU#!mjt9FreBh%$ zJ)C;r?C`~Z^G}CIpX57$XUH|rhdh0u2OL&Ty@$5EY+)QsY(P20;(TMnOgad- zjS-@Zqc~&<;@b$MAI#BLQ{VDV4}34-5zaw8b;3xA37Un`D~ly@T6u>b&_xVO-bh-gd+|5l-SdHe9BSjnI!ew^R^XFBr1|})!#5j&8l04pNo;JX8t1`0D?t+>$!2MBeKZgI5Siw+ zikMtNK$s<{nPzE;s+QmeWSmEDvE#z^_gOabIKo(^bw`-RNj$3G)3^Zn_x%XNKl5PL zskxm#$lEFvrvq-dDQn;@gz44c^m$B1w1Vx3rz7pw%v1_N0%n=VMm%PT&XJdmcC9r| zOXh~hpE)tS_zk}I2ND15Prih?f5iMA&Aj&CVH!S!vuSrY`#4|Z#R0Z<;iEA1GO*aK zM}v=#zDRpL%;bpC=+8>{nI`BI+{Gfkvw3DX|G)9le zCD7#~=K6mZ?2A*2LBX5Qe{F_su^&{dP6J9q@a%m|a<0-2eB2@|Pb@R4z2h;2a)HC( zX09Ui!HjJN0`uz2>z6`1U(&_ zAs(>pXLvi$Q}%y^Z;1iH?b=MO=&)Hf@GLIkG~{J36~sOVqb{yImHv{$fRJ937f+r0 zN&0$sc;PFbK@oi=yY_Z>c-MdWyX+!#+QDYAst(xrbgrC4c@%T82@)%!_--D&*y5jk z2TluA03nZb!{XW5)Q@y>xb*l(!C9nnptB;NgYk}=*JxuG?l{@0m7=XLq_1W;{AFaY z2L{2Byh0LkoAMy7Z$RZ>nNk?aj}N6B^677;j}keG0i0AKkw41U;7hVa_{Wyzh)604 zQ39Bs5}m}CbD|2lqyuQ0V(to7R1LJ^7MDwEv96R+Us|%}I;DX;4$MZ04*D35hx(u- zJu^a6jaL~&CZ7xvAxt|N+3tfKaID*Hn;a4wCUsQvJ#OdY&chXA6~YWJm`IA*1srg^ zXw)4z_(^EnKKZ}Hl~+QU9G!jW0&nf@4X4kr)z=||u)X`xaAW`D!>f0{Fr2*iX`Fg$ z?yBd@pZy}jus1yUuJar*_9Ni2KfL@R=hEH&Ds%ZSqX18 zk&&(4GsBrjK0N%$$38ebc=0^%vHz=Ke^X{<)L`V=-bP_f|M>9o9S#P)ZL4iDe|9_w z_;N{C1S&PZ9_wBQ`_OTxX7m(EA8-XkDSa7}-U8046H~)8PkCm-2BYie_-MwflrDjJ z{?%V3pUPF!_INlu{m+I=r_Z1ucn6=e)l>|o4~N$aEodC0%Kj)ct+E0))9&P?@3o3t z>#r@{?&Ngy4r}?lCt3NqjN*QPGIX9_Xbu25%MPbMupVadCA{Og9~fTv(l>^kJ>Ku1 z`rTojU4k2&?)&IFtBu!QT^=5K++|Hy5x@sraRr1Q!gGKVw0_k0zF*vG5OKA2S|=ui zy7<^nHGyryPv73T@kwCOdC;S_0%HXOOK6c5nVH~y_64;5N+dsH>o^Tdv{se$m6TB` zBzZMR-{ka%d!;O)GN%vCw2uefmyh(__;sXNpPSw+#9#t-l;{bow)wV)1~X0xqGX?B zB3^U95J1G!+dhM=(wTC0jvcO|;KR#Eo*PCC)@Q2eUgblD6i0SUaEWg_HyE+2j%3V3 zz+1n72&3xqzz;Bt)1uJP-1f(W@vO(CMBlOZaNa}AwmUbzh?x#9j4(Quj*dBIdFMsa zHo*(cj{2X;*#a09oW`@Qu7ZM^tIJE*unrD}XFu?v;f+_m#YaJAc)x!>rto{7dk#n6 z-0;ft9KiV$0)s+%*SpX2;tB$FYo3ijcZb_IdEFJp@o z-~JVZUoSX_OMAk%ul%FocmV?NR}_}mG}cB#v=P%Kck2r7U4k`~A~rh_^r2rgHe{T_ z!a03%cDQ^QN8=jx9lwS1^V5vVZza#yT~0(}>b#AK%GIgE?e#~8wYPpdc?OQ$^3Y9J zH$X7Ks~?Pdj@hX>bA%$`y9tN98hrBHk7jC+HFN0ExyYS) zj{XAomEL!K@b3-(<~M$S*js-St8atj-swYl!GQOGw!_IRmnz)iwGU5tFP{EMLtn~r z$=0Pp+1B#&d~e|5Q~x=_U%+bQSNQ*J2|wMG9hj}%wm&#RYvn4dN1-$sNwW(|c2F{r zKrtkk1`@rj8RIK3A?fm8-R773$gS!0Prf_QvMxA(Be0f4d-&if%f+*f+;NmEZA^#D{rH1Z%^Z>b1sFpXJmcm=~Kf;e)tKPr49{oNo{KQ zT;`oycfLJ*;j>>G&Y$|y@YpjyKYZVh{P1w|ttUBV{P(a#UKs9x&-&ff;pCJ5(eV9G zUtr^qBmOTBo424)HsH)NM?F1!^SeKRms-=fmdnue-glKi|I2o6_+gG$^z5UVxpTngG$39M zI(7cXgSRXf2O$>G5stayQ5n{G^wjX9AN}Fs))kIyL%ALu(DB3bA~#%mjPE(1ET)E+ zqol6BN>S=Ii0cC9Ena$@jb1Cy4AV>R9CrC2>`nUUaOQ6eKl_6}4UUz$oBem+`eYM+ z=j${OQR@e*s-W$g@DS}3ukx=I^A^2Qvp)n0LP$Q^L0!sc5UYgz649=8N#Ap8J>i8A z)->V4FJ!N}tRDrCO$QU+Lz`5Z2&z&@4KMN{9N`X_0>^C$zew^gn#uh9Y1$mHS9hQ7 zdO9!?s;$WjMA)gt2RF;BG)@P&f}f6MZPBQkyrIbY`?h2L(fYrlBIV`165{O=3~Noki6w3Rfx zI>9SH(j-i*0%s}nF-|{Pcw~6bv)c&lK7#KNTd#q8ExE*aglQvUgl`_mwpFD_2%$B@3Sn=CF(>&c zEL>&KP^&g&otd6|4TmZt-8l~+j6z|BPa!twoY2(wDI!36gxa1-1)?#+h)yPFMj(Wq z9^PZCUcMD)i~J+xm!B$CaTQP94)rA5+jp;hy?<>r#Sm~+t15v0vY(#EI)v4$XwSyk z$yx%@@(8_Ki6fshD+-&#zg>gY!8gkeuL2b>`xCZ$o_EGQ9NU6rY+xo9PUNE8QyXMl8>!`}P0(m)V*2?C?F$o*N#0$^$`{ zFwNL1yT$&4nJ2;RMd-Lc+`9Ah@ah}yVJ^yHq-;B$#nogH~e@gyW>D; zi9Ny7gQ~Mb*7L@c7x^!vc5|9NEc#-~lLZnXMgq&%gQ$G@KsCfoEP4 zioV>h&AmQ6c*e6D*4Z@mO8CV(Aq~K3w!+V!ekmPn`d&QeP=ioYx*xyph_yl5jT_RK zY0zy160*4|=gb3?#|&Q(dD3*^-MjY!H;=PR?_Uq6&;1y6J<4EpEp=MySWv2sPd)pN z;me;~;5-H_3ELaN;YmG&S>$SJ{5}#~vMCzV`hn==Z?Z?h;&QHRkd#yZoWy z=RWo~aD>)3G4A0Mme23@Ilsa$5W;JB{)=2ixOfqasMLRxL2`CWSbFi_Ce|-m737+B z6B`r2Jk`JUMcsq7!qQ4+YGqFeG@VFen{$i zP@_PF4v;7FTa9Un_NV7Foj_$XC{B*P1_)lee05} z(=~%{lwsu#4_|Wo><>bNTPQa?_MB!^{dhEd=k>osnd)tKj@jMz`SKzHzr8w~TKK<) zD=+am?b$kMSGYpQAQ?Gtr7`}djQATUNOv-}?vJS|eiGbMkDZ!DhKRFYG}~YN4x69w z!k@n+bZBMg?L+Z-aE$GI0eDegE9cT%G7gy<`0KwzwhKQcXTt( zWPM9+`tf}c-^=hD4?10!*4{aszx;z7D*6P9nst8|awsV|>JuYw}Y~ zNmrG26+!+sll)`IAl>olgFEGCT?F|uLl^U z66>$&^_15id357~L6aw+6jMvXj4@SO-owo+F!9wetW{JkLV|Hwj?xrtStqLqI#GbB zq5V{o@*u1b%;Ln$|Gw3k-FI5(>J=;VkDEg_mdwna;Zd@xfsgot&Q(m!*I??I;oH}L zk?Z`h%L_VRc=5lYJ+z;R?ZM$=nd`4_-eiR1MH)_%U0dZ-)6jU21$|c*KJf|H{ePQH zGOKx$@g3(jhgV)_!^qMGuk+GC@H%szGv-cR$KfFTD2iyyt#E8z{r(B)^z*j5l?M511?r|C+N7-pSG2SR(AB<=6$7tOPrX z7zmE)tZHVi@0*+YTUPngDcjp+d-0>v7Ov!t!;q0beNCM2d$@7Rt+-cS;RUP9IYH+&3++u2(R0Jdz+c&YS-hq%$4L4rTXFLBt*e}Kiph(XXfd~~qQipXc^`@5;5_=m0A z>;H%x-H(>+a1i`K2I;JfIdt~mD4yv`i-?6KQOsjQDO17nLf}erl1MP8p-%zIDQf%3 zokVzo%sBwIvc^J;G!j3%>xl=cqzYKw6eEg`KfmhZJVkYF)dqiE`pU)v7~hUenWVNH zRdXSgVl*5AXdxJ)V7O+44dtpKQwtBW*ZvF)=fRj}fD9CwWy4IkX~H2VoE}o0_1uq0 zB@@@v8(PXqBMw;_F=fEv9$xE#9c`N7_MI;P03ZNKL_t)+e63e=_{Oz2xc`o##k=@c zagL09a>^I_iFKop$7=5}q21Dj5A20`UcWtc0jBoz(>pK;EWljDj&O=}$SZ<7TkIF$ z<&&$MOT!zVY8A^&O9UY#EG?;63ulyLdfzJ(dchgf;Q1h>CU^QA z`WsimLq4kUoE8;Hx{`ZXEU@?(bM|grq4^gslTIFKq`SzhsAB%4Pfyp-9(UB5(HrjCehiBwbk6C9HPuBeir0Ny|^( z*5c0HOz{WMqw|akGKC0Cv3tgYFr$t9O%JLf&~)_Jp^fI6W2LySwQ}mQX7e@#O50Vv zs)i#Pu{*CB$6j$7)9}sx3dd{jZqfTa0OP*!wrAJp z;bRu8S?)6G9PC~gEj1|r!q&_mOft%4E#i;oBK%Pi!pD$ z`X%am5MRatqad4WST~O;)UxMXbhz&*9Ee3?pW+|6efKn@|EJ>+b83cbm|_S*qmhE| zw9WQ~W^h>O2i#2OdLK;m*GMslFMsKGZY(c6x*&n=1+1E23B1g1#CH_DC>xc)6q~vB zII(E!|7C;F58;#eW7^~kFKhsE_aLnP`1}7po2>p1@}iXK2v>7<*hlf?d;V5Fu(G@H zuTWZV44?m__wc<&KF+%F2K~U7fv_fy_!!9c2AiLF6R_|75Km2doAi3-`~X!*MaqCi zEPo95y}dAH_?|)$V3SA_Ru@p^OqeNRg6aHBuDqpCa*>fVKgObE=SHAJV`imjt5<7m z90D+pbdSN>iENpbIFa<5zXEZ8E0U?E7%emsum*k=S( zD`xH3VV*q3xgp3;6`j8L2!h9QjzU6PA}HjUV^p1AIKf;OTktOnUH*)9$-~CH(%kKV<%1{d?<~1HzL3-fJ0z8NEXI&hVX0(q&diNM5em9DzZ< zcwCtXneQwp>KDN839s=-eU_PG(~vLb?6P6YQ;wEa-T@x&{$q83*7-8moF^(V71-bY zIxl;{jWcJ5C*Mt*rvI4Te}6!^*HQfJ#kl!L!*Bia>%))yQGzYv<{ssLvAp1=azP51YG%gc=- zoV3gO)7(5G!uARuoA~tb*{{5Fc;u<)P)JMc27C}Re~&K-pP(uus$PDj5LM)f6*Dq22$9`pVr<(*TOUpVu^C|+mt0wKO_(#nzCaSWo!517L2%{{;z`5z3=T>7U0CvFO&*DyH+aNEQO zpk~MjL|o^MTm@RF)YWwITA7N(G@mVt9>{W`0`JPfl~=3(%%@+}>+v)v9UVa%4ndhh z8F~a-9V(&{a|E3qE3V1!0>uS$VjPh4I!K!|=p2uZDlK zBlH-{3ZfReR0LkG&KU7p7k(3s;D7qSM?#|8Z^hW)-e~f+2FD+!H4*8j^9Xt~D$on? zRKcp4vN9TU`u^?oR78sCJ%8aMMAICMoh~@Z#}n`9vqI;!Ils`7i)(k$wm3D10$9oJ zHY1P73A7c9fclXC4RO+)->7hQIof3#!FS$#0E0SG3LqMg-oD4Hxw|_L(?PGn?4UsJ z&&;y!J-^ByUvc%d(#9ww8c|m`*Xu-h+hbCq5V>2$apvZzFqs+obERSJG;g0VZ!E{= z953d~AoyRTUXCB9-%pZr57Jx69 z6BZQQMNTNj?d&L_kcD`f+t7L&3unjo(>YggpAmnL3bn$lV+pV2+8W1FgZKj0!VH|> zO;9!7wB}3$XIo|%^%l``OIQF4oYtdhX^=MzT5EH-)DQM}i*fr9rKciR)ClyS>(8+) zup;KQPJp?OzvIxU56bVv>>q|bT)c3+P!OX48ZBM+3x5~Hs@ODx*N#XcT3k;eU7@y4 z#|-;h*^nKD35hu+x%LWZ30U_Or}s{=rQ0x#HQ)eM764;0v~UB_!TwiyN8=4>%L`!i z<-*bjh8MrSnZsFc-YDxr9lp(uz`{>q1h}|@i%!Lw*AK(B^?z9W-xB2)dDvw;r_S+l zQOw};xWSVRw}C1Y8b$+aP;HeZrSg_yNJJxBDG6mS`*V~-Qp|1M(2a*$F=+6>y$9=w z0$4r4XOP1!g-AgU7+WhPDZ^iq)f)sFDC1>~l870Y*Ak&jmij$us~bm(|=CC~sVE zylG|;=j1i5MWBOn2uBzfMzen`w|eTYWBy+v0>yOn+VH}k{x)X3M`dB!R-+aJPFx*HOI6<1>m1B-8-=Iz^U}*z?}FFZ`?)U z6z_&rQ~>_n7X)muBMhw%M$Z*{UwqFRL)856z@Z8O9`^4nGcNZEK(cHSavQY=z<7qk z`mN6nCm#4&{8na`yH&JX_$|Ic9SAUg@`ERNefw))%85+w@biea@C5@y^s=Z1_Yxdl zgDT6C6_INjzeH-sa$ zT^M$k&tpAYr%Y+Q0BAoAt2~8{Mg(|+PaW3DfX0RzlA7>KGway6^(%a#X9g3V4?*nQ z;ZG-~^J4zEZqEG(F9kBkXUejTGTWzZ_Y^{eW8YmageW(Jdv$e%_x&Hp+&&H(@KwNf zST?(P#&dPL zrowT0bPry(?_3^a42zYh@uJmmjH7nOgZ-zUrmx??)6ZG{EZx-!Ls}2d@Yd!x*uaCi zk74G)tit))t%n#C&JHJh8O%3Wsb`NqU0gXy#ndShP~*z_f9Vauh!Xx9`DOYVB$L$~ z{f7Glo{MiQA-R!K)H2DvtZCd+I0}KM;`BxopW#0Tf^n!c>+7Ov<^8^Dt6x{-ymLy& zMxYu=`|?4J_<{vsGU6;iQ%oY@DC)E5j^m9KiMl$1WK9y=>`*3k z7BUUsUS@SZI>%ub9C^oeXa4B*TD`Sn*M;ym6`qN3%ES5`SeKnixid$w&-ihP?e%M%H zql`R{POyKRfyl=S>>Y$_6h<91iXk)i!N0{-Rf*1zyg{5wS(dVUT(_5Jd@Wp>`RRAu zBk3oX=sC|L@rP{8IoM^@Wc#LODODeo1f$saQ^%B_MpE&D?AlL6!Lh>Us(W!!+0>STBTnG@qlBn$VME> zqELdcol>WWre?H|a||6ILa&fA+*V9St~8;02SyYjgXMHQxomiV6dluhYOgN+mexzD zNwN!*<^TkqeW(7)&fggp*8e#Ir|{E8={xlTX6;jG&M|C|;NZM>0BuEP&7?fSKk3=f^)Bp{o{pM#Oj z%%61qi-LX819?Mu(j&C zrN2krOiS4|*r!nue1F17VWqGk)S%94Y?>A!^|_h)+!?wRJ(07O5?@3A_1diiZXHBr zRAZ?ivY9%FAZybD{L zxfSzZnhomlBCLkmLxOfOqqga#Bbd3gaUrc!(6w~`vWJ@I3cv&4^4jSLAp>6&K;dSN ztI4LL?(D@U__X#_6o{wP*4%Cq{oX!xPMCxd%WB~91Ys(hUK!v9ox7Ta9_RIiuT=az zr_X-hkH3!**!eo6AS0)3(kZyZ`}%GQQgLbFWKIS!_f5V+1=n{Qqj78$7(=0lXHZ@ewBoVS80zD#3saD;>p z{ZXPwg-fZjiv9(=c8?bn_IOKYn^SOh5O_WPuJEj3Y0PlS#bJb;cL;DS>Nn6ix#WnC z#$7-e*@i7%a8Q7>inJ*1vY%n`=!FPR*hBhJSLA@3;svgDrwNn6XEIo>_x326wCjP!8 zUd(}K`^4#07#&m5b$I$BElNGybCox~@iq3Gzc>sppj-UHw5P$GWP7XY>>v2+zYw#3 zhc?8NZc0nP#wH``UA#xTY#&}@)5zI#=Mf@~`hqc32=feCC73t}?Eo~<0o#HP`99$y zp8W+@2^JA*=lF}9RI9$|a& zJZ$O`-vYZ&7wDfk{Vqs{D>2eUbYv_oqzyhGl5O-LKDc6>z(nk4t=2Ja}K*44^!9bOK1zODJb#;Cf=8R3ggWI|UK)2anLX6_@HBfoEr zxk<>8f3|tq%3O3gQAaMLnH&?Yk%1h&xUPPjuLJ!iN1A;r{XwP0b6m~3A=GmqKDr+V)BeC9rn=JUpDD-)J8nt0z4P>J6c%GpwGWEuI>M`rE9Z~e%Djg=mxc;3Q@2rwDo z@lRmEkw*!*tshCOvtbhbJ;%)pQ?|3?r*db~~)0|uB zFq~mD$y|iAl<7F2kZKEKi$Gr=E`A@c*?tlYT_d)Fs)dDe#uM%~TX9317dj!X0?)t~ z;TKJX-)pbTrM(kpc^eN_Fzt6g|Gr^qe|C7+Gfy(&E;b?dxMcv*2`}Oae6cBu4(1JX@R#N1o+@%HVj2-g-q1ysyQwiN?o zjuF`Tw_BZkS!mAnNT~pVu^Qm+nB8=A-_)VybbxT{49uub=lIIGG@G4gIr7d1Z&JRA zvSV(_T;FZVdwBkJAnI#4b%ynAED2W_?9|W*7|xS*&dQh@oXFw3^Y_?-tfJP3v4Pd) zo&c?m>gC9IGeCwMiKY1eT ze+Ye8ePQZzhD~N`%>7nS?zTO9Qdn|Ts71k1TxA?w)Sj9i4YzLAcdMWatr*L~`Ui~D zd#`UyMIBzy^eAC6YiqPo4u}FLFmjjW#l4M|;5|@QIW^R^mFbo_Fru`Q$Da|@;C|mK zfDke#54b|S-6pdYH1MOF#1UyDW8Sw8ECpunT7a2GT`Mq}y~i-WF97W`GI@}NPO30B zQv^FP1P5T5c8nqPc-}BX(6(_v-8uf5;ai{m?cqI7)kinbDVS9#0I8kqHxf2UIE|TqBs?@4Z+B_RHPd*N3|-6Y76(IZ-9!=w(=Yh}9L9fC8n`&EH!L z3a5E@<00PNc!9wMLKG`h|5&j1llK$BY$h3U&x5>`hs4&8NI(glE%5-4W!vgh zNe8bsicI;UgNP*(-hvN&n6z3=%5t4qxtEuGPhcK}?P`MkQ;3_I+|^cc8_?ry1WnG_zOV%`0xFQnxz>=_kQAkIS)|2gQN9###=bX2; z^L7a220r8v|%8SFBS6?0O z@&?>@U)vm5cF$LMfBXkv29&@oUzy$6ZUm$vom6peAo#!gsoFg9otK8;GIRYGZ}J_! z_rIUhW=`jgyLB8ti;HXduDtU#1z*yNbCf*-Y@2PrOP726IcmGZu{%6fy1eC)1{EyM-y^)d{E-001BWNklYL^5szEoT+z@SE~Iu z#w;%`d3fo;{(9&b!gXGi33@Qj&~a+l{y3TBy?khKT|yWpD5EN>lDA!^&v63N{K9FB zI(Js$Gub&Yyz-6TXYa;eVF&0kQ<$^x29_QGoPw!vyRx@mV?xp=%7+U2-mxb>UoC;hmDPa?SDx~4XB#CJZZklXrC1b%K<5VM@APg!l z9bhthG^0e5pfWp^TTaGZn`5gh_PiT~vSTjZZ5WJ^X$MD}p7-l-ydJ@SBS3#dc>B(e$_n!CX*#YUn zB0BFpy;r{Q+=Xq{`rVd%=f(|ICay4V-{+kPUsl5Z;PN62uOGt0D0Dbg@VUguCO>R&_QQ(wyaKjOL z`uw9<0WV^~VG$YZbP<=wyAm+YU{&1H)Lbia;+}Ym8Z3efh8LZ{Ek2-%R0&jA0Jk&H zm}O|HzWUwIU;(k~k1s%-<$VT6{hV=u_Q;Di)+?XVCTTsY2ZW4w4lmDZ_03bEbN4cA zQV@K(id4s)d-;Bj{0n@s$Q_9efJ<0;uJXt$=GQ)sA!b>Nr#wI0xbjlp0yNRgonXMG zUqUNWEMr;z{4HFZ8dxTgiSN10+XLN15I~C^6CWb9!X~-XEzE-Ormboz(mVVzA+3;d zNJ)eq=G4S82ty(26;*Xb=%@m;;vB=d#BsSxqUV57rU^IZuj19_41E}R?Zmm^5NDO_ z>EFN}`dOSiE;(;u5nR9Vr+$8qj`9zO)VKKT*2(4J>4z{|8RfQ69xuLhm4a6XHqyb= zczz$c#M<`-1kf`FmcSif0|#*Uf=pi>Ibd|0Jt#AeZwwjscQ{+1M$amrLBZ+sl#8Fviq#kBR zbrq!N1uQeYx=b5RaFV7|0hcaSyfV@vbWPpBaz~)P11&3iz`h96XBtoknRMYirmX+s zCx~CW`ZMM9fv}8laBAxpzxk6nYJHJc3FSps zIOH=OebUI|?QFN3x9;&~k+hTc$F$M!Kg_Vy>q!A+X3`+iZ9G0+>3DZB$5=M1VMS2O2o4$=khsXUU#7kIG!_<^65Z!M1z`G=?{%YLq7%4L3x~0P1%>*c&c7J@h!A zokHA$ix&8EC)(`HBQW)u;o?J#CU0FIHg`67Z~IM5*_+IJU0Zi8oRO2V`>Bsj9qe8^ zT+jivnq7Bayuoo!IH+dZt}g|D^~~Dv;De7~AzWZYSjkA~G3LIF=ZJ3WovOHwD_tCI zv(!U;35*S5L}VCpMAU2UJYH|R=Afp#9d0|zla9XtC8l!R=jz6yL;m!K9Bg2$GE})L zB9mo>L40>N`ZP;cRGML?SeS%mwi~Cot;}o z2|P8t_4@PRU3)S>;Liu!SuB^U0+B$xOl{ZtXEd)y#M*#8pb39A!vy}qt6b|1PQMLU zr!`-H?MqA>a1LVKokiiFJbeyf_SkukcB7pLo^Jd7CLx{r7m;?!8 zy@Zmjp3j(4KsLux24Px6el^(bKH{3>w*d;w#%*c14A{~}kW#vpp=`*-HJ-##$#9K6Yqg zBGA%A`k1EE&XK_7HLv2MC6TE^BR1p@9x{(irO-Nxy9?9(*jh}XAHp`{tB2q^3yc}@ zBEP{7)Qa#bL+SY2!l`#ayDjU8=B_jM6=S%CxR3mK zNuYi$I;)lsGlw<7{sc#-Ks#G}NeRd1`pv80!Hdwuxc?}eeX0NO~bV0uyy;tJj? zoVNQel)wuJ!AAH>eoOE=!Gp$y4UHo$atSV4TT~yDf0;V0EB#`BNMie_-HW@1l`B1N z7W%}emWS(a?G0x+)9>tqmw-b*phS0|p@rn*I%khMSaFhf4b{gg04g*eNc;W9qggD2 z_ynkQ5`U1bxi{+6QJnm~U8%!Q9JjaF9&ev;l)S@}7J~fe)WE^R0Vj=+IJSQje6XSx zY-oaU<9ORqveyUNO0(Y#J_VArgb!Kmp_h{22tWX^CTrlotxl7xG}m#RiH;MD{OMGt z3qi!}q zc<#Hkk+pl;l;txUkoFafT4oc`)->k8JKJb!q6{jPQLqOIadc8Zfx~@=Hx-7U;~fuN z!>@JvDHIbs(RkixkGUiABqg*AJV{s+Q)0)l1lPu5U^G(c?V-$UqYRZ51mYdR99cZf zRYhPhj~jrNp@UAvZC+H0HAX&#S1Z6bEX!1+W$9c;gzEELZ5z)S^(kkl)_SKv@Eh82 zTJFY8j>Endr|d4P1%}yYo@S(d9_5L+LkQWb$OW4(TxF!W1T9X)61a8!6%-feE%xDI zg2ck=kJX<+%V^PKDpQI;_-4#N$yMt(%}P#u7@)=M5xznAKeIxgF0*Ngl^v!@i@a*B zxlq_`y)BCIfj7)bi zmT6e(O6CBr%DSnq?a&c0*~+|jlS!zWu`?uu;ke4w1sb?Mr)uxTC_kli(wl{37X+1n z7{r8TAAyJmmcxMwJ_?^89`S}7NutCk0k3rQI4Rs$^NK+!6fnTh=a8cQ2#j#RK4EWL z6^ZN5j;LyJJ52LmEv_ZtmwIH7Y~e-7?M#`b(^&;ET2B@o9i>ztCSgPzdRE$(+8h~t zRoC*)-&H1Rc};((3rXjG3vNHc#1}@|ujQV%p$~zZFyv0bP~pEQ;++bFNyh%Dz*YoGZ% zA1d8pMFT(3+DSJ2oIanG6h~f9an_Pkfa?VxMrm+4R=}arAt#VX4-bD)fMr!JfrDwb zVMFVUJJm;iQq5`K5?ATmmvXzDXz@9ac@kF6zC`H7xASde3)qg*@?iWE=HP$vU8jhB zMZP@x)2$2}Pu@T!7Ud>?n4Wr)YI;D{O^f4oV3?hO0B36Ol=UP3fEKJ#~DR| zdM6pv2Vxd1%NJvrzbWfftN^A_{K)i{wx{7;9?fHxAtMfFOdWn{K#CI~;*b(=yO>&qzQe<#mtxVkE8i z1zf3+n9|QFl8l>YF|!TO(A9_>>=DzFw-|Tb#KY7e3d!t6FZ~<_$)|!y)6+bZ119kf z%djrR9%6~8|VYvvpl4X;fz zi9dVs5z<^<#LD9oW;ls^Bv=0ecyeR{Wy*I0n^MmHZbC1f_6@N6eAYal`XtMP4B{Vt zFK=^xjqmv#E)M4}@Qw&Cb)E4gC-!f+{HLHv=Qv{#&SRCdqdy#@`g2J%+tj5SJa=?p zU&&wPxV377Z+7Z9md|`o!oGHw-x9k%wQNt~h_xPR?$pU8ZVA%4s1y(bCO(GjwnU?b zAAl`w4moZDGPP^MahiJY7Gxck~`Y#K9 zs;aVP{`ZB>#3E=1cNb&lK$JCL|(T`s=bD323a?ZPJ&CTX=~aV-uIzS3DWUqx zOM^f*=idtEvP*j8$r*Y$-QrD-u<7(+j!r)7R!AMi9o1v5!ViLkF>d796J|0XX+t+Q%VD+GSpK|MFa%8Xz8 zotfp6-P3HwV7*Waz|XM_9K!19yMFtxI}6omb_)ZsdWn&k`;x&Bou*PZw^aS=OT+NZ zZ&4ppkln?P1s~}=4ZYoP`xg%-wToL2_*Jej8-ZJ+U5u% zj{Q~SEE&QWwaf)GBMfeWSvxwN7n{}8QehmGWt7u zITDK;SI>65$7iyS`J!fxG)&j1Zxct?=I~R)qes6Ii-KwQu!QhZJtxrM!vM^*r#nr1 z!jpaNAseTFp%SPi(R-}cs019^P?3a2jT92~CvOwJK?3Pa=h zk7dfO(=exPI#d1Xl8ysuHIJn!-?$;oK6MjSEdi6qz9B6FZ;e*kLM%A$g+0d-bH3Y% zvQbX7Cj8S8tA%R+`R&Nb@-|+6cK?U;a{yYvT2PrdbcQrI$(;e%O#?cWRDXQOnujdojdJNxpi{FNsOF3^$hT`30Wmc9l?l`(eke zM%yvG0wXCcTY_rds+QCi=kiOu?Qp`r%UcMS0;Pk{Jmu+jI#T!qu4REMVQXqon3kN=}@BtLDz`IRwnJ`|=WrbVXUn-sNi)^aPuOlZ-r~H04UP)i!dsnv3gG5uDfaf{&BtBL_FcYx zxW#MEzH2bUuzLgy`^Wxv9=tbmVR*;GZ$yZXumF4k5BZ3rP|I3a_LI|t>6uy{oI>!A z_&|$zIURB2Zwn*gQOPcy)Iur+$dfYxy}GcoIsb7+HMEL}G7lHCJ5X#wms$nkWhQ(z z3Ycy797Ow7h3m+y5_Q@mOgHf8V6|^m*0#;%)UjFLd{j&sR3=(;()*CS_JVwWFgln7 ze8HurUGN1Jm_P4W@aC*jp9A06qdUzP1iab+@&-2&JXbmZU{x5T+x6j^2fl~3)f z3V|d!f!R5I3>qh(3P45KGn$n;BdH2Qr6A5yOvh~KFWkJkKfvQYczqb3`}{Gc)-pm#Ucc6&LO5hp$`)Oy<|t-|snO$S zXq7}AfJpq06L_R;S}rXpdokoh@`BYexbe|!mH^?e9C@#u)og>iI@X;B3q=?nv98i5dN z+WI=M3Pt*Bsva=H`4WnX#WIp3ZNP%aiVN>A$lM+_vdMXZ+q_h?kDw>O23j}yZXmuG zrv-ZsyEK#YO?Uy@at5IJIhhRTFLNM#<$6jFs} z^c573D8rJ;tLbv0IA^dz5zxGWqZo!SeR*p5&<~s#ZgOnfl;^#f37W}hwSr*xyQWT=jj=acZ1K}VF{B(=oKa{vwOb7vacYQA4lQJ3q4M~f>&9C*{?$XhCj#~ z3jn2~f|m9+3K0(7y~bHc->O*bj~S*yku_k-0|pgQb&^ryZK6>SoR3>#f2IFpEkGcr ziaMZOS z>B%17t_|!ccB1OE+gklAtHadE9LDq9rWd6c=m4QX{6&s|$_Of<$fh7Uaz+>+MBb!B zCFP0~QWId22Me3VGqj zVF-;qe^JL_juH!B2O2-tR=*jKq)TGz$9wx%TE!ZKN0mX_?eegt&BHlR$;rN|9xZc!FuL#x^j=BuZbgb2P39kQmF5Ul_kuQSFFRR5 zkBjCtkG+vAWk#?Pb^lK<*kXn6NZ4PrsIrwQb69Rw2sz3eGtbj=Wqj+6cN}I39fVvLYvSltQ4`+k)`?6D1j1E5Lr~jN-NCx%N8@k5Y&)0Ni3#s3;H= zzue^M!frg0eku;(x}3GOg=27wb#bgNcO{_-PF|E0Gs_FF2cPhdY4k8)R$9*7k zYDI+3A|Sv{XW|2p7}E`Idh?*1d6b71qth>aH`!-EdQCrDIW89LW$o+qy?_W>nPa6? zapT_+MxrwcBl!lF@3YKCSjZ4Y>NSnBzwKrMui!?0+gL&i<7zUQ$A8i{ne08_@R0uH zT6w&hfn?I{G#gj0j*2rmd0Rcj@Z+?C8xZ-So1wo7CrAjHzp6)^ViFPKl8dlL0YSIy zuvYn%dxgd_608emFB4NO$N-_uBYK)JZAY#VXxlih%rxhk9ysBYK%AVjJ5A9v4ZaBB z_NGgoeq6L1iXGw5Cw0RNzz=>Bo4a0*apErV9OYBHKRo--j@PQm2AJfnKAeLwk70J0 zB6;-@xTJOV97DIlf@S6Ixh>w4*h4YgxN#LOWY;D1#Hfj&QcDm$)}6KCnP;9F9^_CGN6~p!BsA9#+a8Dm90%^QoaPJ$yTEPJ@`Q$AlnH6ZdGP5J z@1J;Xqi;bfgz>3R|1^E!biyf$N}zU&)_f)X4?LB3rUcx}DSFjaKwdK~3&-0+s18gA z1E(Z&(5rTJ7Cg&lnm|reR`HKGC{GHFeUyQOTX-VhgK&TnXnNPHcg3%HNQJqhG->Jo zdJZz&X79sTgM?*Tj)_a8cKjT;;V9!M1JN=aAZ`+$p07|dtHSLCBu99wJ#CYXCXs}+ zw{Gb~B1d%*Y3TLTZDs>~j@}7Ubyx|6E5Nz8@O|Jq9uPVQc9OSz9+SDu1dc(v6{udZ zF`bs9c10Ny@4pV-@&&~^L%}bf0dLkJD8Biqw||#;xQB4)9d}#lMN8C8(h)?VVw|b5o}jTTF7daICorWd;wQFD7}g zXZaxtmrkdYHwthVu0csNRfmwgA|q5yl}94zEY18DCUqKQn3>*b?J$jYq=6+VdYYZb z3MYTyD!qM0RaflX8s{6z2aIT!_=t&?jQwJ&+mW|XX6vkW?DBzy9fUuD^BV{D0`#Cl z*r&~V*YW-*k~cUDH7tx__>JEqKfyFbbP4C;0%szy$QogDUxRb+xj9~>qU|b62sq5! z7pjKwL>P2uki41>%#GAL5gN2l3#qPV-KoN^5Pkd|$=tR>cUW z)@uIK*Mb6vD1gqa$q0OO&q(_uCkc)o5yn&=Yzi3wjY7K@xItX+OXdMXQegaPsyE!t zn*uG#dqTmfd=+C{30ITxRH8C!nOnIM#=r;$4d46jVFlYlCyZc_S zUOv~7i+5>O*eXxtsXAqrqHo8cl!v(@q*Ulg?AB6WLH4UaE>WoLK~7L824gbY3_n_ zz+Z_84r7A#G~cMAVOp&NP)>3VKZ$6@bo74o4kTYGMmE zxX$?_H%!n?ikOf@9W}=A(uEge7$31VCQ5w0}IW1f=(bnHmG z#m<|^H<5Q}uO?d}d8*fWvf(wSSKA7w>M5Zf*LVN3bs3~pZ?DgerroUexq^t4F1LO| zl}R`7oslMe001BWNkl}GK|pCxrZ6Ir`%DSKyhV!+D2jsg;L##O(ox|64;uccbc=UHnlzMCw$~yI zo9xKhV`SE0xa^KW{)RK>gNt#l)LgjyQ0B@$yJupXWkqRoh%!v?N-l0(s9Iw6B#tsP zfmVrvzrdE>Eb*bhX?@!7NG;R&JPPs~e^b!;nDSAO$!3>Q z1P1XcVI8LNZ#dt_`=6y(@XoR`*OUf`{x#)i*3(g_I8#3B!LQ?#)2TyXQo_&6^;)gp zU~dNxBBZtV-O-*5|35#~KNJ>V0~!vZpK*c&9^VG!0muTc;9Wf__>-RSo})9p?K7C$ zX3KnnGgRf?@CP$G`gm({p!3tJuDC>!I11!MGX`PegxyV>!06@wT)C>_K3p0;C?fapS229AqNRM!ozQkq`vy zPiU4&39uuC#I`OVQ`S}%%gD^l4%}ri{};vY?#rBJ@{uj0Mixxa$tGDX%7}n z=!JcdA1XFDSDs*$Sm~-l%ajG=B8XN8rruAp-j>~nI6n=R55I78ns@VKn@}!aQ_`ZK zO!~}xrbRJEDh6O|A=hpHlCF{YowOxL+)xL^yvo<#5NJWK+HI7Udq`d;Xz)0?z zyd!^kQWu>^gza^Cm3IluN4h^LmpjdU%xn}-PFZ$J;oP5gxWW;O zntZnOp1j93KdsNQXWux2%A!K=0iIHTYmef&w^g~RGbmfJmdPT*vPXmnHfos;$wX_H zXuZO!VBDwzXq_q5ixSa=qT)-FGG5a{VgMlMYOK}sj|a;HHfjA@Eh^!UAe(nWGmC++ z`qgr84?Vrd+d)N~G2P1PMGksa5cgnE%SGD~VSX_#yu3Fb>`Z*}OXS4#G|D8GSMLlR zIj+!Jkha+qj>_wAeb!Z1OQd8gE;D*N>*sJgOz0NHY!{L!KJGZnypUX(5^*(6(yK4T zqT^)y%EA*iiU`4XDumVmkAmqn9MG6wa0YQPh_Ck%e}HF1Wu)^Rd3U#E{oDR!wAY?N z&iF0UwKg;f1=<2Xstgz~Bes4y*@PUNWs-JpiC%x!?lX_BX>Vl6d!kQ0+ccrS(N~N`R7*%FbDt~GUMIt3BPy>*q3`AD9!Q9OvoFo$9Hs$s zd!r0o_1QsoS~0!aRyL@A9oYDq8akeWX!reU9csI1aiNf2LhX%kS^@6zm>=_$BB=Db z@7h`*`Dan`na6*^ZN<5ImkbSZODDf3qpt~%GMj_!Bb-}Fi|a=SzSilp)e(g6oYWE5 zrwUpLZOPB!g)rF%ea@TQy)r6R7}Ex6ok|Lx^-hbJ@V(amOd*6VpG5e%m)WC0VAG{l z#F%F-klh*f2<-VPv}^DUdGh)@L1c%N+gXsYV5lfUEAXxXd3Kd~}(^n_8wmk(>e@!9CYrFca6OhOVbKl)^z zt_ZjdKS-ktrVw13^Ez-qnVt%K>U0_*k<|Nt?rV@qXd{^{rl&x9=ox!8JSU2uWaFoo zNfzM9ToFR8s-^`)EJ}88nZHwqeuKc_UVM9SROy<|4vBzfr0;ACFMx*jPtZb50W1=a_*qC->Dwxyij7QFd^I^-R-qJ(*bJcH?1ZgI(4WZKBN(hk@jWPsAx2+>tz|! zGxCFbaN;L@5SX1;tjvu3P0%+e#3PeT+5G@YX^UIwy=nJZ+uF+d?eqWR%@0_SAx&z46_nn_7gZ$QfsG`Jrj zZAn4yghK`0rCoJ2Np;-!)zKf#MHsPwi~ zjIj!TW$JkJ=YF;PWN_D80j?5SVdnLzx3(+R@idA06*CYD<9lT;8|B@B+cr=UhK{d= z6ARh!q;pUO;Geq<^R^wzps=saMbmu1!!woo=+r?fSPg8hH9sGdD^t5hq&`@5<Lxs7HgnlW)m$>6In<{OMB?^NDKW z>N5TvZ@%es&eRa!K-~#gmTKB;6~4+_2bksXRXW#J_wOFQ?`tkdyojj)Ss?b_5j7G=6 zYW$=`-LLd(ps}2udQwGm9J;2IhAjj?@{Z+%gi#r8LbNoZ;wftzy8UDQp@}dRDhlUK zK*~*2Q$REH)1n5W=t9y&=xJsOw2li`d;s@@ta6iamAjlq@UE?+jKbu+Nu*;1G@#D^ zY`giBbpjD@&DP_iz0*<~U<(?8BWg89B|fzVCU-Vk99o@v@k_f|Lz#?qSHIy~!P|Et z*P7R-2puO;$f$;(RLzw&>4iZr3gA}xL`>DjT-;HgwQ$es&%X0XWKy!q+S(4KFSW|H zHBA5+WWJVtR1mdn6pYgFD(Pt4ru=`dmQa(tlFMbMF8qJ4&pQ&*#rs_S8Hb+5?&_y9 zbsCAYt9w2FWxpZuFf1&3-_0di%+{N{DDP)_AG7x+xRoHx~Er?)+C0a}0P zBJ2uQlTAlGBG4<}EL}GWlkF%V#25*H()w!BQTE)JT+XP94-(w^T5}50>4uCBtpZ_G zz80Y%5I25OFvb=2`*GbZjgeYyDZ^P;%9|we#*9G}e4S?rRr)|oqX%;efRCYbYif&zdb3ClJ`TZ_-c#Ph*dxv{b!qIqWFNy zHCtCH?mS-#r&Mn+*HOP(F2ZjPL*Wvb6*0!Mp;KUduuLE)r*!dO%J#Y%3@;(Ob+@$O z;8YDzz4o!+guY`ve9=FYfrEkbZT$ouG-6%ZP~^)u2vf}Yz-@y?>b3vD1|d6GPj zQ;hd2T#}W67F7wID!qKJl_ZVj)xL5fW&-ToE=7Gc-cYm!Q+y-l^Ix!u#W3#g3GjiX zm8YFgus{){o=vapK_2SGe6E^$ma1g*)NXpKdwjH%gobb`gD@m>B8c9Db>$zuArrkF zLjcWltZKq%LWgzTi5UF5aST@FGNE#F^$aOQ~q(eL;)2 zkXU{rlU{Wcf~z~mar;cQ7q}>^TK%9<3!%eg!)-Mk7CZnxNvrDEDwue4jp&8dr($an zmQVf>jax>dy-RwoN^(7o1~?U{R+Z6Yjzg8JN-rCMN{|YS725fu5g%fuV6TBkLxVNG zSJ0EEXDoQh0PUTBjSwTUGok6$A9$^g>~N*6h++;x2agiz(~(NJAD7BX8>}9vodJ8^ zPT|thwAQLZ!f}qMYs-cf#@BsuO)sjtJ=3d+@xlOz`FhCcA2Yhg)U~ph84mG*f^Ozv80G~a4_+^PoXhJh&g;_| z@ypb9TBqgUn|uybw%anb*A4utn$d5)S!WAd2%hE8wBBsj)%3^n?;De4wR9}1?NXww z#xPI-Ne7ivhPDiF^bJa**RM=<1<)Oo790ux2%Tzwud&KEzH52BG^r_8;kWbGxMj$( z|77xx1lMj&IcTvKgjQ#Cs6tXggw%+wp}n9PChk?<3B=ogq)p~AQS(fq@Mrd(-0xGq z{g~Fn$V?136lga;*trUXgcNS!TFRuC?bm|WiDK-y2Df&`qUo7O4eJr)H9K{b5*U{? zA!re0*SlZe!#mAEb75K)SBf^%)CWot89MQh_B(qd(>< zq=4^$pbtI>1i-O6G)j)~kGNIlHVQU~rF4mdDFU?(p(<^aL}1A)luio(RNa%-OjJ;} zokl*TPOU$M-v<->)eHs5y9zu9hZO8lSPT&^9Eyz23Oj2CMbn^Aw9)!fxy$6X#Xpx0 zeRk)iSHac*v1F6DtmI+{QxliROmHXCuLEabQgQ{#*hrjhd5bEQgbg}(I zwW-){j-t+*rYMXVFVHJ0x1(plrNKZ4n8@}qF*mW#O1FQxI>I50xB!5qQS6I&H0?>Z!KZ{WvzXg5gU{lf9UwS893g!$64x zW`X6>nX006WYx<>&@g!TQ#M@REox1L0|=A9w^brZr#y3`c^=EAA`7urG7tB~1IjWD z=TzNF5Gk>u;35D#_UkB%BR1tzPUN^L$8X^t#LSE!T5V0&#Id##7llb^^_!{V6L8`t z?mpzln&>($7U^R-0UHG4)t?^K(t1=Ji#>mEl1Fi_qpS@2xLQ%*1X|+t>x|+j?mApE zsazs2qex1O3z5S^K?MrObp|yuQDW^&3=<)Om}dbMk*S3k^4bXYnTv2~r^Jp*w=TKA z{!wb(jPT?!Ba+nweorsP59DO_$3R9HqqAeIlb{7Lkxx(}NXV|}%$^KYc7&C2J#=j9 znm&RsnxMYt?zi4T1Bytm-iO{NG*FXi##g>#aOtT+^6J0i!&VoYF=Qkas4?Eh@h1T9 zy%H=_X_OTf!k+?UNLqSAY37ONDmGbO*oF=+)@`iQ z8szLlt=06yC|BWgz@CD#Sc(S%sL^*c@OE6RfXeFK03NJ3Rj_(i^PvnSf-P>jr6FQ=S zX!)82pq8ze%+3U=rJj0J3PxX1#f*lzKKIWt!E3MIto0d<8Oi)8NJSyVtzvOGeFFxWLVsIIU zsx!oEk)t;8EEy|6Upjx}ybCU-REB#oeR+b}%Sx9ssv<|z)Y4$d=i-x5+zKY!;Jw8X zUFB2E_{qRy0~du^+t$Y!#(ZJEk{eS~9r#2Fp<}0sc@EtYt^()JcN7xLOGVbzm8h7s z)UZxv%&$p>)~HRnG4cKeD6G+%$uFGvSCg+ybK$_4rh~OlB5&xBDhFWv=RWS_>KQ7v z@=~71G*$qF8pw_GiJx4xM63rQP+&xJLA78u8FC!P%0Zn_Qw0muoZ7ST^h8g97!~a_ z>errN-i}t`4P2%Un$R`p?m5QSQBR~`hg1{^Lj$PF0-HQ}Zy$VJFtR;${CY3fH5tlI zX6?}puha@W*(!n#z1$jXu=--E-e4h)AH4XnMj3rB)x2UC1ks zeP$$ov^n=F=yTl^4GZ8=`t|>->`WA8w~g(p;Yzw1?EL><#(qv@Ys_A2SApohS1$?9 zX#%LZ3Ijoq&F(nd*s=<~?gD7va6eX5aOwtjB!_CR1LJ)aVm#bKvYoY)!#EgxC5Ond zS7L$*-#z#*x5L#0wLyr7YoQXY6w>?k84h!eC(jB933`_I1y}3QkPj0`23i8dgDU^} z=RfCmQw4oPr?gmson#_yWv>EgPy#$KX7}HiFi(OIg4If`^^K#rFgHfU0izMUY?FXP zQ~hr_3%t{5m>`Iqre#3g$(4xm_?j$f>^erYPSm$u_iTb+kYSmzO`YXwU%VM81SAL) z15C&q7O|6V!JG_W;^{klh8Vr3#!ZE&jb6|EQ&w|eaxL$%O{g|J z{6(rKMeNx#_O_cd%NUXOE~kyI6H9c!%Zj;^3c8J|eT_4-W~>bw_;>`)cn8E#CJCb; zqSC0kfeDyOj@&u(F@NiTf+4PV&?*C#b-s0!MzOPXE1dsh>qS)gU{H-2VYP9!l#-!v^nWe+aW|4i`t{z;Nvd?LR}%M z{B|OE8cVz`AM`7C+k~GaH$^Bh=0O*{;0ZsHXUG|V#&@FCSVkTNd~*`FNEKuB^LhF) z(A5QAs)ZpSAWR`Q4Pfg0ytZd6&%7n|CYuI?k&T7??__~~`{oF2d1Ys*&NuV&2m3_e zsP}g3fi6#SY`p_?B|*0+94C{Ucw(H`wry)-+sVYXZCf+3ZQJ&VG4V{Sm+#$o-}~=c zf3H>DRke5T+Ev|a)vl_I%fXWtRP2clX2ZXdT5X;L5DZt=&cD^Kt73J_CUYQzF>yI5 zR5f4FMZiFRm1W^+XI0MEMs|eG!id#uOxl4X$i6~`5Cm2PhL=ljg!srloRx5DAM&Gc z&k!A`HWU)|V&&t+L9}-17QG0KUP!v4H0i;L+8CjLj|>JLW{}JTGy4HC`Ah_LmE8|# zIhBdNg7wMSA6sBQS5DFmK9ci+uNH2`26!G;pESMcLERx4{VBf?>b)sD{baATCN%Q& zN1J#IzmMz?wB0+^aPP)Yo$*Lvw+xb$-wZ@7U3jdeSv&zS(1qO#j`oEDlIx~)QU~Dn z(H(A{t_z9>Z4JPT;NJsRM+XW^Q893pe!3XLe%?heN*>`aEDU?SIYWmS2$P~-r*J_g z@?w#_OZB*@3<_x%2&mQA3GurMHeClg_&9g@SQ=l&I)I|7i?Jbh21)#tij)DFp$o1ZRMSJk z;OMXOcmbiZ?;c0@{zuDdy+_c<5oFD@&16D~>0dhxMg4+)rL5)2NohVUlir>WY|X|*11|N3W#ts zA~?C)VIBlLL)HRC2gpXj=9!@8V3%||t(kZrV8wC2|Lg~)j@B{!tI~MjxWmZ*V5I@47m@3Oe|x=$UxTxsm7tO z!)~pfOW0klBRw~+e)9nDAROTx9>2usG)mzxP6@Hz-)sWFr@e_pM%ZMYRh=&KP;zcz zV`=%5*W4N1o|}JJNp3uLRqzn&VQPjUID)|4LL{ophFzzfYDu_V(WUZ)oU=bALf^`@Z687W_ zF;smsxr7>)ofN)#$;lC-bbISxmkt!tEi0HimRK;{O|a%U z@Zy=2o3|ET5lxel`Z`x1>|Ra9=+zA>`568Tz2|=%suWJWvyw6T>)-2<2{bc1!J6Yv z#>AalW+NnWYvX1V=@Wo)g9~g~FF=(s3?~YtB#2R?1w4R% zKY5_a^wX5K%vf`={T3$H-x8_ckqFcMkY^B^>WISg_m62SE*#dzNtey;HcM3XPIEMs z8A zO~GifxJad9GmYM3T3x z{Jyk%dVsqvnZk|3GBz+YZlWP44_kM{%efTtn zL&c&haWU4?fjtv<0q{h&eJdNQLdl2M z%8WVi5HI=+zbg^zw#z8(Xy18d8b^K`k?DG$X+l!nMuO(e(&)%#<^>%8)AHb^($~~J zoX9*a61w3R660#P+aOcR9?|cbZVW0z3YbYWD(QD#{fw)zZ zC`}e<3gV4lq-MD1c;Dc3(-hzPVxqy6$wWLZ@zn)L6wwnRn%s-$c0K`6{P2YvC3oGA zPeqy$OIC>D%ORzp-f*ZczLb8!hqzImF=DZi**E1feS&MG!<_OP9SGGzd zsJmO=N?|)fcfp4u;lNK8dh)KkGrNtRw&F$x_CznZb>qw-AZgj(FlDIXVm5{cZ>@9X z>Sar?&c@7fw>-(nI&W7@%f-e%p#1{^FK}5!)%KL?mbkE;K!YKSJ=c~;EO7NUQDwgR zQ4T2e6j5GTlPbJDBke7o;wCYz-{w!qt|yBBY{l7b`=2C7tz~v&Yfz3k*IX0_h^Yy zK7z#jMG5p!`cF#*v&WIl2oX^vYpUob04A&+b6l0Gt5Iv$ROK};W{=mYI5^;Kg)_|O z)&c79#hcgi`rSO>vLr!*Y#vUoigDaB4&o3r(N_EcKAy}X*g32rU79%USjH3c#I`y* z`XO7`GdUVFX*jLL!|l(T{m6kXK+(n&^Zs;UC!}p@9#8P^H`w((H=^Ge)Q7+mGx*w- znyBbzzq`lsG*otf)?J40L_uj9=kQvoTp#fWr2!MTfZ13#UlOt}bwEXd5yaenB{Os{ zKB>ky&fNA7iy)odO_EYzCcN!>v0Gtf%ig4Ze5Z_5OyR*vZN~|d(}I4so6}tB&1%t# zdT#Qzf(OL>(o;dE;60pF}GG74p4&p`GyHNP_TvAD13^bD=aTQV&{Ev|%=Gvm;7{=0ElCl0x&r~KB z)Oy4+qI0w+=fG%D1|9$RgJ^$?DyQz#sj&iSPlJH|W(|oXVN*lyGADi`FzzYS5tyQ0 z*j@`?+rIpYT%TCwoQGPy1iNCe;wi(EG^6swP&U(q*##cgS-G<%vLa7)*80CaTTbS( z2c0%OVdJ3!FfczeeLzq(4*``A%|b~-KY@SrFz;^vjhU+4n>u!)@bDsi>?gx#6m%4h znBk3c9)-Z{x)nhVo9?(NAcl$}&+0vL94g0U@tZiv8DZm`TY84;dQ-8V))Ti%FEX~o zmvac8zmiU*BDwkm&k90BmgV9Pj1h@HvIdadGA&UH&NMfr$;*M#&x=fw7Ek8X=14tc zT4)KIQBG474T9?4qw4#~?p^h-G(2@>LWA9osJBZ@NN`Dez<9ko zl*LWZor3z%V&ySl(mESbf{#|+ysLrsB#TtBVcPx>->H!1PrI|h3+OV2e;Mot#{WCK zx3DrnA5qXY80T!aO6Eys6mY(Nweiy=%@w8L{Z^X8ye8gkYL2fD2AzMPk=Pgy1&n=w z*C`<|{=F$78X6zi8KY|j?pXHk;kOhDeNL>DnFdV#kT!)G#RO78p5e^Bx0EN#-TGpn zhWNc*u!T-xW#SHWMF76_>+$+Q#0=uChuV9&?W@|XOhNp-`)Q?gCVo>0Jcn3sC7mra zFvFJ|vPOOWHS4!S`?H8lyXKWljOAaWRk}CUlnHB~2qsTtaR;&}!dO;8Y}dLC-0b>S zG#29QpWnR>9X&^*X5@tutWY6^salFVOvQg@v2C;FP+ewDndvy{g&|qnGOuZtkz~3Q zs3A&qlH9c+83o^Xs1%IqKQ_JABJq{Sp6GZ2(gOS&!%f$-C&RA=KMEFikEa>lv|~Fb zi3jZQC3PH*LVkX?4X*%KVnb^i=sSd(SI8GB)>pBagO%w@Kvqqv$KKeqa76OcV98>{ z@Nr^kC#Lm^^*z-h1}({|QpdzqHR(N&9M`j=B-?MsL3B#YmnIs^ z#2Nh|2q>*R#0@xPSPs|AE>W`0FTNQ*dtc93QH68B@tqkF+!3ZWpx7f=vFk#fCY8))@5&g9PL!hbjDv;Yx%MOu$U zM^}ffW1$ew?J|mtl?=M^7ZM)L;*8+70^@|wW`&Oy;~IBv@?IA}7wi2+QuG4C?5UQm=i-Zgz@<1j^CC5ml}*KU#fp>O_CFo2HA?3}N-pfH+nZ@fw!4vo z;K?mvk&NflaUo5q&Qdr2X5fg3^_D!ZGyNc?f^L{=LevoyVj@BP9zB}>LZbSvI7M4I zaPH;V{iX`@@68DnLuZnOBo`Zk{5}^|@5Mn73JJ?fyg6(+H6nD)*LJ8Llr}I~N7d-N zb6SELPM?UTE|7gumcuq0!SDalTRm&}u9}IEeBT|37Nw})m zz(riT`9GZ_22Qu(js7!9g<89nKgSl@6Q*?_>IYM%`53M%C^*(_28TE5pmI-RwgbYN z*R7>Kv`RAs0i?l=sSY1MwuZx9ad#aJY0EEC`$f$k5H>jRd}*_>3_wuGE@!wdw+J>Ky-rjr5uLwV3!7G8>WG+ z5~TLRY>bQ|Q78hhh!X*)>NY!A8lAkY%q&N6kDX)Rl~RzRgFX8jWyuZaZuid9W{}o7W{S!BLc_!QZoA%*cTWfw zU=V8D&j(2OGExX2Rm0Nuk40tPJ@GL>8qd&c@#D`yYJNb5s9eyZv?jab@ewS2T^=uF zZzJhK0q zqdr*BAv8Xo?z>P@q^ZAQ>FN@{PUXp%(03<^m8g}gzB+X6H^Slwi)-wFaAM$@galCo zsP`yR-m=-j=>Ua~N#HzyP*08f&f`iHCNu+ctrd-RNZ3b96eEUd0|g@}Mo(imbq-_RbvxaQn$huh`rH^$7cF% zt_kz?ko^3CjGVWcJ%1JQew_u25C_G79rxud)pOP*2MlW%pA&)sJb8cbegu!Pia7BM zY`|}Ug=hvEt$6prl+aXEnQ@f8nJ;uEEOBaP8|H4+#NhEwfNB2&2L%@-)1H`q?(7r* zTHd)dI3(M_4-LU`u7<~8Kqf8u{DJjQ%u`8#hL-tzTj~&Sp_PI8jiIuw!q#6m*R`|* zH~9hmFmXj+WBzNk9-*|}B81eP8J@}+!l-pd5_$U*?VXiAK_E2(=w(Z3CEAr3`~!sv zLSLM3B+n*Ntt(s`^Ed-;c|k&oYgoI2Bzi8!*b)NrWYLlN)boA$MjUymbu?9JZcWWj zxQ#FGv@Yh2x_9-1UT_gm7@2!;=7J|ayl7bJc_p$0vv!(+E6a^&`rgL_vYFJ=7a%jd z#wZT*QnPcc@#0bqix*VbLp@Kgjyd4`;A4eMfw`dX+4?#5h<>*D*^v+?abQzwr7UT;>IyOKeohGV6ATG$1?H8t@80< z1%NI+qmPAWsHBlPAT{V#F*Q6B>3UwzyLU#`fyVh_AYNwRWnMKK*8snd6_JTSv?r4m z*vZlHM8wLx%RJvw&bYP=n?;kHOC&$VvdK$uJGu*P2m{=luVsSja1j|K19+E01=jBo znylf!#tIQ|z^C>Q6i9j88FA2bAjLB@*hM(>z8^;eKOY3|f>FA;2Eq^**0&|5e?wGC zAuSRMvrqvCNr*eXspzm$K_^HK$`P$dU>|e92q~+pgKns3&GNZ3BCUAgzn83-$ewoW zf&-DkOB=-D0s<`$u&obreHv@rx+fxc4KQbHDuhn%8EGwe$*~fgU=m1B!2!wJj z7KZjG-$2rHvA1pt(1^2%=y#&9HuqkU0kHuKShlq|e81o6EhvqDSQY+s6C*$okSfB^ zsUh0W%H#bCe=e%oqsAKnZ!WU-0H&gQ&~uzz+>bbaxY<(a&emsN)C=~fqO7jgGY}@| zLT8@!W1*VUmxL0|h_OBxmx?C|Sf(1#htcd8>jUfak8~nK8HwN?L$z1`s869c>k=kS zxSIm3-&xVED`gu1&zK7(h{tkAL*~%8AWb?rQ^`&&agvFzMh)Q33a&RvuTzEDd5_5V zd!f#D^j={MG(Xjo-3XXEI>|}t&|?a<1Gs04+RmBo*L^cTv_t9&_Bsl};jNdUc3oQagTpv_d`@@CbuLa!0I2C?yBfC1TTu-eEwf~8~rFC%zd#()x z`vp}s%%MgD6!d!>eiSqv&;Rw5MHF!IxBRV8n#e^>$kma9Qi)JvKJzz<0eAPrV3LK)~)(VNsSPpoyMmR?cM3kl3bVK))Z zS{CKO=+&`M5t!sBG#qc<%@>@R)V<@c?3&Q!N=AQ@!*`L~T~vZpv4JJ7#W+a5XTC}y zo?keBxXASy^XZFf{Fh6Fh?KSyN7DdG?53GR&2NUxU&=HwK2a@a7#Bqo%tCX3`MgV} z%xw{tUO*~X$tzCx*UiFLg66xYWT>qhbi7q(?CAK0k2~C~2Pz=A3X%H?)_OBL)F!j{ z8Tb5!W(G>#)B~>DRnBG0zST!rk|$8afgH)11CzBm?+kbGEo6cbvR>-0(G_gbDa9HByQDUf692|%I4S|%azsDlqqi@ z2MJ+)=Pno*+U(|t26%mMqOX0XkbtB{=Dn-2!ZCyKUXQn(@#}k^M-UY8{uq-ieJTnR}GaA5*M1f z8Tdr{DhxU-?YOIbzcLxK`=R}EeA4mD4*=K>&ZQrT-(p+FKmn<34Z0_nqjG3ZlVzkF zjeEcS3r~cLy`GIr(%MPjeJL|v7?N{*A0HZ6`S<)Fp!u{g9YFZE0^2~I9+~joE*(Uz=>;%sQSNZa~!JDJ7K0B{p zk)Yi&=lk=!HW`zERRjVe#!t`ivDz`uNG+V%JbzoaeV zmNQ!hu*Ka}lDyA7LX99#ad0aIrS^=hq@;zbs$BS~&?1;5qs6xLPKa#`? zkO0@eY)p$TyVcektqos&V$zY9h~=CXgc^cqE~@AfSVz#R)Rmt3|SV5xtwjrY3_C zwIAwko}mn}OPS5wE2W`!rZ=N7mW8Mp0E4L|gcj<#SZNHp0Zk7b+3w}=OaQeCWtio+ zi%G#ux|6Ddbxy=mvva`D*0iMgmL0~BzhhyOA(Z) zY_y6|=)rdCMX%5EuzR4}07b?H$Qvh-#lm-BV$*gFwy2s5vO_2S#zX?%rme5#cH@^n zC%?HECvCzdW|tuh3}_PVB93}*E!09m0?wcjx*>^ zciTqZrpB%xk#jKV;SMtZ7;;PzH^{yIn`Xa1bNNt?Dp7(TPp#@jm9d!>1hft0@}shU zF61mIk)@SWOL``%ks`${%HV4gj1x~m^Z;clWOld>gLJQcs zKibAG+-aEVd*YxBWVHA@Z?QJ`CaeECqM8Zt@ngtc-e&Cbj9iJfr{vo#p)J9^ld8R| zRLI$9b;}J*zVqJr@EqKXS1=9E>Q6{Y=g$0{=VnxwvRO6=rDBUQh38mL#8=Wtb0CI2 z@-V41z(-qrzt0XNHzm5$^6^EsmBIu{ofS>~xZM|@G73$gwEJXrTTAbK$wDAezkFUi z1_>P2KYYHu{bdEGa~2oJo-Fh+H4QMYbLbPan2bF1YMSv2dlDr)e$GIyWLro&m89W- zam<06vuW=mfKwVo5p%x+js|k0JD5#JB{L5~4`m$75 zs3gOk$J2L|Zis|Hx;q3PffTkC5A0>kh}`S6w%844l;1;;Rn7Aa$$S*ON)C*lx%9uc+Y`H%4mHNQsV3n2*_3o(lpXZWD`kQxc!nbX*jDNYn2detwkwQ6`k;A8nR8g43n6sI>HRhZ4aLl4Uztd*7Lz4MoeL6 zI1y&wRxnQV#K{1hnXm1!1U<-E(`4jX^mPD6S;f-t>GoF^j9(u%qqoyKAD?AFs3Ig% zKDy{oKqIOe>K`YVLL;eW;HQLS|1bTgpCkal8d_%mD7z=FXH~>{|6wmr9raIK-y@&P zv>|nvMA7^^`yBe~a~*rwTDL~16FeWK!vo$m)diULlqsX>n>!12;Q`5U+Iu4#z^@KA zoBKo@J}zLoYrHYDP-{%WRiw{7hC4~po&2=JutkMzFDcMVSS%y|WC zH|#k5LeSLG3{h(8DM8;AL;aF<3TfFym9lV(?XEaltaIHhaT<7J=hKYRcnP4c2_L2& z5PfV7lJ6yLKg;TRZ;Tn8StAl$z=WfZ!H1wFTy|bS{U*FXqQlFlcFBo#IVq+D@$6~x z{kp5K^=qm?oBDGlRc2I2@~MYfUC#Va7pH@iMyi<0L8{PTlValGrJU ztYHN97}6-jqy14^_INAxI1F@#WQY0VSf)u79+rBVi1<4~4SaeqPkEam;Jm^q?)g~2 z2M9rDwGj)@HPKWOhs8uWWJ73S3xNy3uHZiH3({DZM-!sMrIq5UC+tXAJ2ZA!a_$07M9<#H}31~P3~ae z^G~8Z3o$>8_TtwG3YyEs4!8xnOIBJ@dp~Zl*THjP`qrf7@UJ`W|IIbM3aQRvCMfHr zx-Coc1?RH@$JbzL7;Z%4rp{r7M9e3Vk+v1H=S^?gv@;tM{{nUyI>ywCj8AT)YAN~< zNGX4oZ9eg@?R)9Q;kl(_`JW7TD&rnJ^RNIHkavBrkAi&3dz{BX=#|esVpO@<1%vGR zNDI6eu1y>ov%tq|K_R@Gojh{Pk*S`wZ+nfmfm9rHaUa)_l)*loL&po!nzRF4XXV#N zrk+hAS$fMI{kQ}V<8`^WFw5A5Y)DIg*8}Otu?N>ebTlWnZmsxwU-mm~d;IGlL)XxY zOjp4R%kAqj%6OSgBS*!`E|3H$E1`b6Slw^}1z(!7>VtELB(cKs;!5_ugwjHJ{`~+( z0Q^MXmv#{b(q9@*gA84KxyfE|SL{ZwG27tUl13Nj2GJPx3CHSv@9e#<(-;D0G%61KYxqRTZ zNM#?B!L>8lD#oGD?=Sa-mO3Ck{IeDM)6l>|xd-8HI>OYTu;p&eF5l#dz4tj_*aD?= zq|DTfs&W4heMj>+cHB4;3)dw+wj;93IiMJ=b+>7&^ZVs2E#x`ncH8C8{x4g7So7z| zaU86eW}mB}5EK78-mvr*vne}V|2flM{m$AomPqf>ur`V3@YgEjV(GAyzk)2oOkxE> zPx3ubUS%zYv?i}&PtJWm?9g*s-T3c1^EHre`o#HSDC){uW6)2U z4mti$v>kSqz-U6tx54XYYTiezwHP^G5Npi>MJLY>$0s-Z0*hoOm)82N__NXzNE0D? zzsV&j{LwND91N)eK`beVMKSO7CBYR7)G!-Z0E(7!@mA< z3x+Rhr9hIAa0baozC9G^1W*i>R1`d(x<(M&Q`P$!QH4IPy5q6cjw|9(xeLDPp|HNTn9WvnC`b9*LJJ8uQ^Rtz4!~jTWCvMIF3k&*^G^wS<;~cvzOw(am|f% zRckg3{H~T)gr$?;`^8K+l7jZnMvK9EXicfU@S}$bTB6Q7EYlMyLhR%nJ`;P*@nTRg z{-`#oMMCbz^?aNjH+VGykN0M)kW0WIMmrqeu=F=n2*!en4Fo@3Q^OmCqLA)OwaYlZ zX5?*4q{QH>Qgw;Oq@*hT<)pE;!ORg(Me?>Io2~EP&}uiB5B~aTz67NF-oMa|PoV*8 z17brZedLgC%;0ZXP8w7>*!!Of5!{0-zp{SWiFDw+HfYU?N<>GW9f z^opg4%$7UsTmSrt^4?6vW*UmxPJ%*9)+%dXtJ+PDa)<44QfMhwJr3`}{9Ju9#9Jqp z<3P8JP!B?W2VN?pRD@)B>F7YzW(4DMaCoDYVST=Eoq5azdB+TobD1sxT|dL?Se_W^ z$A}C`d@tblp64)tX?=gRNYx-J)FBsNt1GmecBAz7%mRHCJC=QYy#qHq-E|%j^UdC! z6LIh;qslxG^14)v684={q1(lj-2H_KttD>b2LZM?lC^MRz!FZKT6%z4bQ;`yiLScw zR*$zOH#4yqH_n?D;-z#s_-84VpmL zn^Qmc5nc@h7Zim*TTWOs#J0?wmi!INOquMlP!>B}{sdH~S6X3%CJ*;`2|TCtndnSG z36-2JGNE?Xau6|uVp{o<+AVT+BWdEvR3Q0~mp`rt?b8Sn zU+t1QB~2ogT{(hnG2`MuY+eeH#6Oz+YVy&j7p}hCa-1!BW?Jfb6Y+f z1?xM`G%afJXEEORd|)IN7jLgKdT)ReqnJXK{?3LEv54aj<4diwJT+Lz#b!u0#45iT zZU6B9rKNJ>KD%W<@<}yQd6U{o7#!h2=DfYP?vXmW<2Twx{93xzF802oEzi$ZoN7D< zozGsTicO5vTp~34dQI>|uoM55Z?6lJ1CbOwFeeQ1R@&<(88qAM;j}V-KQNEhmBC)15mQe79y92 z?b?tMg3~Gk#RlAl>ZRzyKWlmxB4uY^5VRD|r8J+8LA#S4OieY)f6yT}vBE~)di)C= z$*NrAv0A5^@z+cSMdy>UHCdl9o<+J76#u3+K3fMnH3u&KLVRg2K`_qKx+(nrDo6fr zA?B1%znaf9S)I}>?E???oYQSNO<)w4>gg;aLhpGj_=QHMYoEGn1W!#eDrnzNYrM zEcXdtbL9a1t&4grk~kg1XFAIN1#h>*6c{@xvI$NOH6kD3DLzNEt)`~d{^7ZE%_(EX$q%Z6LuzYYJh&P4*>*i+QXW(9 zPaDnx@#1$vrMBfq<*;+0+GEaT+OiX*+^CFf4vURnX`Oy+(2xG4BBw0Q!N-Ao9l(Ag z@ilB=@kK8KV;=)kB`9LsLiXRpauvct(zd&Sm|IVvzUo&_Ep&`e)e)LVY_xbMb@G&u z=N-3%i#|`mkDi5EWV*11ezxEccMM^|%aZtom?dpA`vOYn#svSeHm5O4^=mAJkI=^m;HGoDv(&Tx_4m zU^ALr4x@Fn!_HS@h-Tvm+HP#U)oJXoUmh$>8_x?{P{mo#BJ9Ny>p!= zb9bVwT8M{tK|jD-X>joTCRxyfDnk$F?q|e|PdqNMacSfE`F@-o`*h*J9Za{aSEjK-(4ek?N^kC%++yHi_tYXe7k&|U76nTbo+V5LvtWIH!n zOV@(N`AWZNYHzz~VCYL(YTZ4r27H{$BPVMC$HMv(x-F7YN{< z9GtHL>mjQ?`{nBt+vaWeHMmXLi@m+4&RXm<9jD|tM^Dv42Bs)K0{LJ=-pW7}Hfl+L zVx1aq0G%u1t~WVA8-!mDDtjO>n))rv{c1Z$ne`@coUT>T zIhr8*jRNKY*T;|8j3Q*G*)Vh&*?nz|)+FbGm+T&0M|BMSHg{e^);E&J4ZgkkSATi_ zsK#cx?#j~*e3~@_x5~I+IvK$Dn}<5lfF=oasq+2~kF@EaGOlHR{7AF|Wl-rb#Ei!) z0@G401jSP`^-|-^-3#3kZDF?Wk`#hnwdB7oE2}ivExXBu^f(wNcP-T6 zZL|QoM<;uv$%P3&0ymX=KvHDx4^)8_8Xkp+(dX3O1{u}v^f5&jdCuD*VPH{yVe6#^ zt#Z9xua|wM4xwsQjUP>4{u3X^53kzl#A}9kxZ+g2O#F4$0T^|v

fQ(2=B+CU_TD z22FC`w;mCdP=NCIJCXAr9ST@Q_?gkZ$A(`@@d^0q@R>-~=7ixh?N8a;zNX}0Jjvhv zKC*V}hvIe{3QtEoY2@PHJ`QF-Kh`;IMHP&Vr)Xr(%GLFx{&MU9gXH*wc!Y)Y`N+{G zG7l1{68*4IB*EPM=CvgDf34*b*Xq`#2G<;&^{WfI~k0| z2b(z_?nOzAusn|j)-u>F0lCs7P2u%KdJERHYYct_>DZyCO z77nBi!_RBvp_!kU6TB&WjmdxyBu6|c!Q?*d0bqvReDFekocO;_COiZ?9Qq}?K~w5o|Y^FidKU<+BS}*Tg1?k<;I=;cFa#x z|48Gr8v1AftZQN71>N_Vty>ndqi#A}troN8S>Aw0*RXM(5{ywp53vsu0*~>|>+IJ2 z6r+22>#fk_CLdOWN5qCBB%#`KJl)1w3KMZ50a8vIcX}$*UmObom?1ApkYHg~_pS1~ zaC1CLfYA93irnbxyHVPRM6RaDOpn361;jtttWaWghxW-rw4zS{3>cgqW<( zqzF$R>J!QgOeS^)zCNK%22)?f`yByI zXaW&eXYlzqe_eMfSg!Xlc8*HwMNwc)^zBaD(fpjT_i4xGT?%gc1^kvm^V`e0_EY4S zZQVkf3e|mH>Nt+jM2E$izPRIG4dYr|H`s^(q znhy~jzyCEWc;LOAMC`rzoiysYTmo;XFWN2hJ;0X0f2b;5M5TT@8CmBbo$EQ6W6ainf7m0_j zt}_S6yv9{Rrf7h1CqqH;TI)wR3w4IU-R_@)gqE9JU52z)k`y0%#k5y`f0IyFyBUzn zO?*b|>f%`Kg7VQI(8ltKYq zwM61Ze$#1oVyqPDPi~sucKgpXE7|>&7z}{yA6r3X-WkWpZ>DCX&IeC<=y{=`7bSuo zvmk;%jF#^Q-sKGvXjYwQ;XL$BwatkxFH|C>%B>iplW-z7B!nlFNFt@ zEuU0Pv3w}>kpdr7)F`_PxXZDp=IE)57wr#t@P~jhmOFTcK-8r92=338!i`Zt$Dn{vccEXB?OBR_a z*v1ax);<$w{R_alJFGkj>|&{;9kZ?ZOv)$!%ZqS(>vLjyGa?ksDuCDZtDggV`PP2g zPIwZ=JA~g2nkO5vRD1FY|6KONDl?iQb}jrBqH&ZAId{^oJ$N@Sgmi+GvN~r)--Z6P z_6`H1YxPw@z-6aBF4}gJ&Er+;bU6#sl`XlYu$c^SX*)cgPQo4BTM1U;5^{$UgQv7- zQu#;WRNrr}e6UQS`O<)F@@$z@m^`5}im@=ED(H|9TDcQwwcyzK-E0l1*H)ltO#bG04|#=-du0tuT*6Ggl-?ZX>d9baMV( zI)OaxVGXKM23GPp%#NUl%lkp;j-F57_-q~&K?xADQfTQUy=-E0@;fsCg z0;Gu&8aXaV^*r$6#r0eX+|O+Er(vX(l#3bI(T%mp|Eyv{QOf_?c_?-D?62lmMENt} z!tdI47|h}S>ut@;Kcpg0Nh7_2g5|brK3B9GeuAEq1Ca=ZrtiV#k<%Aw{tLPQT+t zQilACda{%W0&g$kcJ`rw!B=oX$KgU$(9XoOFtphpQ2BJ%f0X|W=;tI6=Bz}XO8dSt zcNbJQK_KkiB8*i44&2^E*BGm}^7?I13B?tqz9-;PAN&*;FO2$h>NRm4OrONsxwj=o zC&%{>ghktw^v8uF($8p+2yZTt8*8w7`Q;vZJ$vhdH6jq7avoz(2e@xid?Q6Dxe{H? zXka$fLx+<|76C2YlBmd~CxG=M7sd8uOoB#wnAH+G1)R~(2WM?Q^|43GpiiAtVLxj-tKdx!P+4qa?@$aH=ZlG$~y zHjO^GKSzl=pe)v8Jd}*>)jyrlZd5Z-jr1%gfE#`1;aW4_!>HYdMH-(FHnQEh=9+_v z{T6XEp7p`2mwmzp|EDJbUDT2b-0OK?g0_n+!#jK$D_87}l{y${$1S|&4EXtEK?nt3 zv?>BlGT0Pm=j&6<(-I}^smNs;b8-KNpf+~>lO6bDP#mFk zkA)j)`o`t#RNJJ0Hg%bhP>>GQsi@vB3++LvdP^u#HC75fBFT-fqSX2||x#gF_?He>qkAylHwXXW$kM z_N$Lv%(GK3qnf|JFaK52a4PZ6Lo7_5XQ(@tOJ?=iz{slTNy4Hkx*e&DUx)~qf&W%- zCz;NZJ|5*nNYK|IsbqVEI-4rtcNo^f<@MuY$}UavWn;VNho@uDgu%KZfJWbQej@^)*lQT~sUR_Im5ZWFG|8r<2A_+!xFt)pUkEk{p1}WROh2<6ccnrc}N<4i<3K zcu#M_X221!L-~p{JYB!UX*+o+)osO~B=0uxPC6@*yaF>ZIh@(I11aCv8++z_Tbegv+ z`15n}X0&t{5M+RnFA740P2RQ5Q!)M?DD~pMA}h3btUI|+%tO1UiZjMms`Rdk49R!R zV{mR{Nc#Mk-nf0!Ye5TWJ34zw?EkaVm9A(T!RxVrJ<9sGCki1$LPhE&W{PAdaW2f` zke>}4E}nkef4aESZ6v&aTr={lgy5gM?_~CYUbri9RmjJ4{O4IIWSfB#-drnrxOIMa z;N90xBpGMmcia2%IBW0*eEsG0+w$AcFR-smT3kV_M#L}({3{Rtl|*b^%^aPq>|MZ+ zze?(6mR2UVX2Pa!N=EMXVDvyRu>T=B5G^o>O zN=8)^Co4x6Mqw)_RVzCNQ6s1SP^Sb#`oj3X?W=h@n*CRelA4y1xPgL$lbw+*80Hs_ zw4I}^ncWv*GgIlWo0GYbiJ9|P{eLkgX=ZQcWb_65uV&T0s{b$1{|Te~KO8Xrzi$3_ z_P?M1r{@g+592?q8i-PYVSmw8aJ5r0bF{TG`A>67P7aP{PA*notU!Fhn847#?*E?w zP%<*HF|siG--Gu*M*s}%KeUWo{#ScfAKO-S#jjfmZCJO$m`eF{3I$qd5<6}_O4a7W zbxP=((xe?Hl&s94~E7z4GknVP1>e0gf#f4 z+StT|Gzm5)w5k)GvfuB%d)|BTdoK=ghN_Ai`Q3NVIrpBAd+xdCK0hm&11T5%EDVN! zuI%U*PZhOUGx;3AN}Z37HbEkr94V?LXhx#=!DLfwRO4t zag)e&5uuj+YiepCrkj5Qm`&DEeYIe`F0tDeo4X#hPvuog_2SyC#?%3oR{K>^L(6KL z8U%b$jRIOk4{Owrx&^HQ=m;%?Hmyoku?q~xT4gxbjI&D(8L7MwvBP}v>Vpk(;1*KC z%YmHZ(d*L&vK5vo>3(I*$gs*_gd9c+sml$;7T9kPcFd^Fnsz^CU{Fmc8GWxxsDtR0 zhm{>#XCC~qi#!s|ZMK)1aW+GO66B{>?AP;MfJ7lx4KKEGX5oA@qSw^lwkusz{XYO~Y?N~i` zLo$x;?|WR)V#O{^SAYlHsQPuUG-RS@cH=XO`#fTT<+~w7D~qEaPG(4IhsQ^YM8YGk zxt8FcVjYLSAFj^Gc7drK+sK_*)IERhb?A&=s}3TIc-9uo_dfVt2s^tajTa*kI9UFb zrf-{SS1l@r*=<$p;EU_vPjT!BEr==2_-j`Y)rBju-MA<9dfd07?m^pG-?i!~GP`4- zGg~ubWj2EKz0l~JfaRRH;pVkhaV|sH$t|nas*gm8E%3bw9lf0>;f6^>3zmJkk;svo zTI|D`&U5PpK$Bu z@CqUEnxA35Kf&7XG}`Y(uEMOQ9ueQ#BkN6@ep^EJGNe5y1q$Yl ztnUG441ZRiGHPaS_1IGeH9QTfM(i4!bv85#OhJv{uO4fQI}5pSMX(){5Q zmsLpSL>jr1{gb)ZbPG%7U8{wqe=oeV3vx4$r|d!SE9TjD4%o91UV%s46 zl~rvg9-qc;!~8I$!lsW{_iW8}MbF@D*TFcs?RK397vX0-$vL~(Y-sDN&W2=j5oBdi zrc;aWY)7o% zS+$55*eRgosf@ zmf<|B;Q33Q7lo8$*R!KuYa|Y~v*gJ^<8ij#{2Qu{C>ikdQ-7tc6VR2Ka;KN`K(LhJ z@$}|acy1JtnrCgs=`Q$o7a%-q$8n9}k5PTSj@B_ywSu|}(DmTeR8@Y-9Gw|+pSBux zuIXfg&S^4YMy}O6Gr~ooTn|_8rPi+iGxahPK_bBZdz-kcY)fI7Dn75!jvbkWTn;2Nsx&A3?+tnDB_& zJfb#_sL^7&l=KHnhN#UiYV)ISSYz|#FLV~av`Qil7ZJ&*0e5&(I?yix?zV?}RME^0 z6Hd)qc5BSpa@wsbxi#B*i*>{{j{TdpMLw~=W$Rk(KYgb8WKVo$=9wmz+!>QBKV!Zq zeQTM0@z$AUpSVwEfBt7*R+#=Dqll)=f?{99KK;Ms)YbXnvik1zG`-%t*^S%z`D!dr>3UZ<`>KS?797%^ZV7} z1$-w!mwQb;YU*=LjVG20`_A-o8l&;G11Y?J&@_0zOx>?ef~Lp7$We-WEUL2IAUuru z%EB!{NWPh{BM8a2A^L-m%Yfi32rM3+G1Dj={3ns_c=Fqbi(g@iv7)%t&}1 zh_0?6l%0)->?{m-T%{E|1oq1!7AOk_P4qW#Ma2(+eG& zr4kcTmskjUrWia}dacTP-LdQBdn5f1^}f8}@Lvvo@68XzVx;7(A2eM&;(rJSPZ^~6 ziEtBQ9r_o8h_4QDB~oXWO2P3)mW7zT;E$f#8eR12#7&R>;pls-E`0k+x8NU1Xv)W8 z3r)2daOP3i(B2=MI{I4g=U@EC>r=-XmK5B=c9S{Eq`o@q{Z7;NRCgGN2yA=&zcD4Z z8>B3H(GEvos|ahnh^I^CRD|%5$SM*tZ#8Yb3k{5Bis4*-I2A@coGB&;v#H^5GB+GP zn8{{=DTN19;h|&};D);Rcyc71Pp89$Os*I%<->#JOm;Y&F6V|yc*`uA&6Ex`=*)mT zyrOU~XYpc&EQD<}alJrC?|QTUo|SuE>HE>}@yYj7Uwhz2GeD=Q#Za3}TXmQlF&LX% zmMkRx-Cs}qYWwj&_x@x<>CmoSZ=CjnaUba`mkQ-lJe5t2;3YryR#Uu$xG09*U_$*0 z=T)Z94_^t>!xKnP<|n97`b|FDE!9S)oGiHPp7r4^2z3w<{FrMx7H$a@73+mO#ibV}|t?@@5+_vGd z*H;~%(}G%P?8ddQ|HzZ4dPXkW_Tq_?%O~=mIphbMzXc_8YXq3Fn)QOB$glnUpLKis zU+8|S-?6C0P}pE)h>eg5?5pE?HKI^_pDLP)L4Y)3m%_s4t%(+0M^(}5@EV9J>h?ovH zH8d%yg=W^%a>(=;H8ls)g@J3r+$T-VbV-*7O)VMGq-j@8OmDt0x9P5@pWXJ2KQ{lm zea%nbcI(E-#=@}s?yA$5^bfti<*5&ruH3!xZ=HUyGtN`o%Ft94no6z3heZp_-;FS{ z=I3%kQEbG`R$KSnmOPL_63ySIa4_G2xNG?$=%Bs`GT$0ybtX^ThNBe6tFyeZbaxw*5uCDGXxi8Ob%x5Oh+{rfNydjc4r8@X2`_eYj9 znJ}y+^m_#0*2H~(t?A*>>V3rT>w=JcXnmDI9x?SQQ(F&b86*0;@g)T6cq*MNXG_!W zA2QUFrmhC#|ox0X!?Hfm~nm*V#7Cgdi} z;vs}JrmnGYUL@i84~x2ZdY(Yf)duM~@iH2^`Om+TZ7{4SO)Y!!W2Wt?9x#weQ+Jx$ oI>4BQyIOA=lctuLD7cB-_o8rReb^99n)>78Oop%Ax8~db14!PN?*IS* literal 0 HcmV?d00001 diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 1323f7513f..6a36f898b1 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -163,19 +163,19 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, switch (level) { case LogDebugLevel::Informational: color = FColor(147, 231, 237); - UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); + //UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); break; case LogDebugLevel::Success: color = FColor(156, 237, 147); - UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); + //UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); break; case LogDebugLevel::Failure: color = FColor(237, 147, 168); - UE_LOG(LogAirSim, Error, TEXT("%s%s"), *prefix, *suffix); + //UE_LOG(LogAirSim, Error, TEXT("%s%s"), *prefix, *suffix); break; case LogDebugLevel::Unimportant: color = FColor(237, 228, 147); - UE_LOG(LogAirSim, Verbose, TEXT("%s%s"), *prefix, *suffix); + //UE_LOG(LogAirSim, Verbose, TEXT("%s%s"), *prefix, *suffix); break; default: color = FColor::Black; break; } diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 9b69da797b..917ef0b0d5 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -13,7 +13,6 @@ #include "Kismet/KismetMathLibrary.h" #include "Components/MeshComponent.h" #include "LandscapeProxy.h" -#include "AirSim.h" #include "common/AirSimSettings.hpp" #include "AirBlueprintLib.generated.h" diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index 458b4c3f95..d0b449de1c 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -78,7 +78,7 @@ void ACameraDirector::setCameras(APIPCamera* external_camera, VehiclePawnWrapper external_camera_ = external_camera; follow_actor_ = vehicle_pawn_wrapper->getPawn(); fpv_camera_ = vehicle_pawn_wrapper->getCameraCount() > fpv_camera_index_ ? vehicle_pawn_wrapper->getCamera(fpv_camera_index_) : nullptr; - front_camera_ = vehicle_pawn_wrapper->getCameraCount() > front_camera_index_ ? vehicle_pawn_wrapper->getCamera(front_camera_index_) : nullptr; + front_camera_ = vehicle_pawn_wrapper->getCameraCount() > front_camera_index_ ? vehicle_pawn_wrapper->getCamera(front_camera_index_) : nullptr; backup_camera_ = backup_camera_index_ >= 0 && vehicle_pawn_wrapper->getCameraCount() > backup_camera_index_ ? vehicle_pawn_wrapper->getCamera(backup_camera_index_) : nullptr; camera_start_location_ = external_camera_->GetActorLocation(); camera_start_rotation_ = external_camera_->GetActorRotation(); @@ -95,7 +95,7 @@ void ACameraDirector::setCameras(APIPCamera* external_camera, VehiclePawnWrapper case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE: inputEventSpringArmChaseView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_BACKUP: inputEventBackupView(); break; case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY: inputEventNoDisplayView(); break; - case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT: inputEventFrontView(); break; + case ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT: inputEventFrontView(); break; default: throw std::out_of_range("Unsupported view mode specified in CameraDirector::initializeForBeginPlay"); } @@ -201,7 +201,7 @@ void ACameraDirector::setupInputBindings() UAirBlueprintLib::BindActionToKey("inputEventSpringArmChaseView", EKeys::Slash, this, &ACameraDirector::inputEventSpringArmChaseView); UAirBlueprintLib::BindActionToKey("inputEventBackupView", EKeys::K, this, &ACameraDirector::inputEventBackupView); UAirBlueprintLib::BindActionToKey("inputEventNoDisplayView", EKeys::Hyphen, this, &ACameraDirector::inputEventNoDisplayView); - UAirBlueprintLib::BindActionToKey("inputEventFrontView", EKeys::I, this, &ACameraDirector::inputEventFrontView); + UAirBlueprintLib::BindActionToKey("inputEventFrontView", EKeys::I, this, &ACameraDirector::inputEventFrontView); } @@ -217,12 +217,12 @@ void ACameraDirector::inputEventFpvView() void ACameraDirector::inputEventFrontView() { - setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT); - external_camera_->disableMain(); - if (backup_camera_) - backup_camera_->disableMain(); - if (front_camera_) - front_camera_->showToScreen(); + setMode(ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FRONT); + external_camera_->disableMain(); + if (backup_camera_) + backup_camera_->disableMain(); + if (front_camera_) + front_camera_->showToScreen(); } void ACameraDirector::inputEventSpringArmChaseView() diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 83a0275fdc..5a4673d97c 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -34,19 +34,19 @@ ACarPawn::ACarPawn() UWheeledVehicleMovementComponent4W* Vehicle4W = CastChecked(GetVehicleMovement()); - // load assets - if (UseDefaultMesh) { - static MeshContructionHelpers helpers(AirSimSettings::singleton().car_mesh_paths); - GetMesh()->SetSkeletalMesh(helpers.skeleton); - GetMesh()->SetAnimationMode(EAnimationMode::AnimationBlueprint); - GetMesh()->SetAnimInstanceClass(helpers.bp->GeneratedClass); - SlipperyMaterial = helpers.slippery_mat; - NonSlipperyMaterial = helpers.non_slippery_mat; - } - static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + const auto& car_mesh_paths = AirSimSettings::singleton().pawn_paths["DefaultCar"]; + auto slippery_mat = Cast( + UAirBlueprintLib::LoadObject(car_mesh_paths.slippery_mat)); + auto non_slippery_mat = Cast( + UAirBlueprintLib::LoadObject(car_mesh_paths.non_slippery_mat)); + if (slippery_mat) + SlipperyMaterial = slippery_mat; + if (non_slippery_mat) + NonSlipperyMaterial = non_slippery_mat; + check(Vehicle4W->WheelSetups.Num() == 4); // Wheels/Tyres @@ -178,6 +178,7 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se else EngineSoundComponent->Deactivate(); + //put camera little bit above vehicle FTransform camera_transform(FVector::ZeroVector); FActorSpawnParameters camera_spawn_params; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 184d6d9e6b..9897a28399 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -65,11 +65,6 @@ class ACarPawn : public AWheeledVehicle UPROPERTY(Category = Display, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) UAudioComponent* EngineSoundComponent; - - /** Whether to load the default meshes */ - UPROPERTY(Category = Display, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) - bool UseDefaultMesh = false; - public: ACarPawn(); @@ -163,23 +158,6 @@ class ACarPawn : public AWheeledVehicle private: typedef msr::airlib::AirSimSettings AirSimSettings; - struct MeshContructionHelpers { - USkeletalMesh* skeleton; - UBlueprint* bp; - UPhysicalMaterial* slippery_mat; - UPhysicalMaterial* non_slippery_mat; - - MeshContructionHelpers(const msr::airlib::AirSimSettings::CarMeshPaths& paths) - { - skeleton = Cast(UAirBlueprintLib::LoadObject(paths.skeletal)); - bp = Cast(UAirBlueprintLib::LoadObject(paths.bp)); - slippery_mat = Cast(UAirBlueprintLib::LoadObject(paths.slippery_mat)); - non_slippery_mat = Cast(UAirBlueprintLib::LoadObject(paths.non_slippery_mat)); - } - - }; - - UClass* pip_camera_class_; std::unique_ptr rpclib_server_; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 3e0fcbb7c1..d17507b9d5 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -11,17 +11,7 @@ ASimModeCar::ASimModeCar() static ConstructorHelpers::FClassFinder camera_director_class(TEXT("Blueprint'/AirSim/Blueprints/BP_CameraDirector'")); camera_director_class_ = camera_director_class.Succeeded() ? camera_director_class.Class : nullptr; - //Try to find the high polycount vehicle - //If not found, spawn the default class (go-kart) - static ConstructorHelpers::FClassFinder vehicle_pawn_class(TEXT("Blueprint'/AirSim/VehicleAdv/SUV/SuvCarPawn'")); - if (vehicle_pawn_class.Succeeded()) { - vehicle_pawn_class_ = vehicle_pawn_class.Class; - follow_distance_ = -800; - } - else { - vehicle_pawn_class_ = ACarPawn::StaticClass(); - follow_distance_ = -225; - } + follow_distance_ = -800; } void ASimModeCar::BeginPlay() @@ -61,29 +51,6 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) //we will either find external camera if it already exist in evironment or create one APIPCamera* external_camera; - //find all BP camera directors in the environment - { - TArray camera_dirs; - UAirBlueprintLib::FindAllActor(this, camera_dirs); - if (camera_dirs.Num() == 0) { - //create director - FActorSpawnParameters camera_spawn_params; - camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - CameraDirector = this->GetWorld()->SpawnActor(camera_director_class_, camera_transform, camera_spawn_params); - CameraDirector->setFollowDistance(follow_distance_); - CameraDirector->setCameraRotationLagEnabled(true); - CameraDirector->setFpvCameraIndex(3); - spawned_actors_.Add(CameraDirector); - - //create external camera required for the director - external_camera = this->GetWorld()->SpawnActor(external_camera_class_, camera_transform, camera_spawn_params); - spawned_actors_.Add(external_camera); - } - else { - CameraDirector = static_cast(camera_dirs[0]); - external_camera = CameraDirector->getExternalCamera(); - } - } //find all vehicle pawns { @@ -96,8 +63,13 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) FActorSpawnParameters pawn_spawn_params; pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + auto vehicle_bp = Cast(UAirBlueprintLib::LoadObject( + getSettings().pawn_paths.at("DefaultCar").pawn_bp)); + TVehiclePawn* spawned_pawn = this->GetWorld()->SpawnActor( - vehicle_pawn_class_, actor_transform, pawn_spawn_params); + vehicle_bp->GeneratedClass, + actor_transform, pawn_spawn_params); spawned_actors_.Add(spawned_pawn); pawns.Add(spawned_pawn); @@ -122,6 +94,31 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) } } + //find all BP camera directors in the environment + { + TArray camera_dirs; + UAirBlueprintLib::FindAllActor(this, camera_dirs); + if (camera_dirs.Num() == 0) { + //create director + FActorSpawnParameters camera_spawn_params; + camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + CameraDirector = this->GetWorld()->SpawnActor(camera_director_class_, camera_transform, camera_spawn_params); + CameraDirector->setFollowDistance(follow_distance_); + CameraDirector->setCameraRotationLagEnabled(true); + CameraDirector->setFpvCameraIndex(3); + spawned_actors_.Add(CameraDirector); + + //create external camera required for the director + external_camera = this->GetWorld()->SpawnActor(external_camera_class_, camera_transform, camera_spawn_params); + spawned_actors_.Add(external_camera); + } + else { + CameraDirector = static_cast(camera_dirs[0]); + external_camera = CameraDirector->getExternalCamera(); + } + } + + fpv_vehicle_pawn_wrapper_->possess(); CameraDirector->initializeForBeginPlay(getInitialViewMode(), fpv_vehicle_pawn_wrapper_, external_camera); } diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index a878672414..60d453beab 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -37,7 +37,6 @@ class AIRSIM_API ASimModeCar : public ASimModeBase private: UClass* external_camera_class_; UClass* camera_director_class_; - UClass* vehicle_pawn_class_; TArray spawned_actors_; std::vector vehicles_; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index a75a8250ac..9f06927099 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -15,8 +15,6 @@ ASimModeWorldMultiRotor::ASimModeWorldMultiRotor() external_camera_class_ = external_camera_class.Succeeded() ? external_camera_class.Class : nullptr; static ConstructorHelpers::FClassFinder camera_director_class(TEXT("Blueprint'/AirSim/Blueprints/BP_CameraDirector'")); camera_director_class_ = camera_director_class.Succeeded() ? camera_director_class.Class : nullptr; - static ConstructorHelpers::FClassFinder vehicle_pawn_class(TEXT("Blueprint'/AirSim/Blueprints/BP_FlyingPawn'")); - vehicle_pawn_class_ = vehicle_pawn_class.Succeeded() ? vehicle_pawn_class.Class : nullptr; } void ASimModeWorldMultiRotor::BeginPlay() @@ -101,12 +99,15 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve //if no vehicle pawns exists in environment if (pawns.Num() == 0) { + auto vehicle_bp = Cast(UAirBlueprintLib::LoadObject( + getSettings().pawn_paths.at("DefaultQuadrotor").pawn_bp)); + //create vehicle pawn FActorSpawnParameters pawn_spawn_params; pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; TMultiRotorPawn* spawned_pawn = this->GetWorld()->SpawnActor( - vehicle_pawn_class_, actor_transform, pawn_spawn_params); + vehicle_bp->GeneratedClass, actor_transform, pawn_spawn_params); spawned_actors_.Add(spawned_pawn); pawns.Add(spawned_pawn); @@ -137,6 +138,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve } } + fpv_vehicle_pawn_wrapper_->possess(); CameraDirector->initializeForBeginPlay(getInitialViewMode(), fpv_vehicle_pawn_wrapper_, external_camera); } diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index 29bfbfd82a..7f57ec130d 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -39,7 +39,6 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase UClass* external_camera_class_; UClass* camera_director_class_; - UClass* vehicle_pawn_class_; TArray spawned_actors_; diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index d54ef67bae..52ad1697a0 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -62,6 +62,13 @@ void VehiclePawnWrapper::onCollision(class UPrimitiveComponent* MyComp, class AA LogDebugLevel::Failure); } +void VehiclePawnWrapper::possess() +{ + APlayerController* controller = pawn_->GetWorld()->GetFirstPlayerController(); + controller->UnPossess(); + controller->Possess(pawn_); +} + const NedTransform& VehiclePawnWrapper::getNedTransform() const { return ned_transform_; diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index ab52f97d62..d8449549ec 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h @@ -92,6 +92,7 @@ class VehiclePawnWrapper void getRawVehicleSettings(msr::airlib::Settings& settings) const; + void possess(); protected: UPROPERTY(VisibleAnywhere) From b8ed712cc2da1707787236d98985d985a12a5053 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 9 Apr 2018 20:08:18 -0700 Subject: [PATCH 058/121] ability to replace existing pawn paths --- AirLib/include/common/AirSimSettings.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index d81bcb280b..967978cfe9 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -461,7 +461,7 @@ struct AirSimSettings { msr::airlib::Settings child; pawn_paths_child.getChild(key, child); - pawn_paths.emplace(key, + pawn_paths.insert_or_assign(key, createPathPawn(child)); } } From afdf484299af836266ecc77e0b15b60cabb6ea03 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 9 Apr 2018 20:53:39 -0700 Subject: [PATCH 059/121] remove unused Camera property, update doc for pawn paths --- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 4 ---- docs/settings.md | 7 +++++++ 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 9897a28399..3c12734269 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -25,10 +25,6 @@ class ACarPawn : public AWheeledVehicle { GENERATED_BODY() - /** Camera component that will be our viewpoint */ - UPROPERTY(Category = Camera, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) - UCameraComponent* Camera; - /** SCene component for the In-Car view origin */ UPROPERTY(Category = Camera, VisibleDefaultsOnly, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) class USceneComponent* InternalCameraBase1; diff --git a/docs/settings.md b/docs/settings.md index 487022e922..9c2dfe25ad 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -125,6 +125,10 @@ Below are complete list of settings available along with their default values. I "AdditionalCameras": [ { "X": 0.00, "Y": 0.5, "Z": 0.0, "Roll": 0.0, "Pitch": 0.0, "Yaw": 90.0 } ], + "PawnPaths": { + "DefaultCar": {"PawnBP":"/AirSim/VehicleAdv/SUV/SuvCarPawn"}, + "DefaultQuadrotor": {"PawnBP":"/AirSim/Blueprints/BP_FlyingPawn"} + }, "PX4": { "FirmwareName": "PX4", "LogViewerHostIp": "127.0.0.1", @@ -201,6 +205,9 @@ This specifies the latitude, longitude and altitude of the coordinates (0, 0, 0) ### EngineSound To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. +### PawnPaths +This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your pwn Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"/Game/MyCar/MySedanBP"}`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then chosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. + ### SubWindows This setting determines what is shown in each of 3 subwindows which are visible when you press 0 key. The WindowsID can be 0 to 2, CameraID is integer identifying camera number on the vehicle. ImageType integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). For example, for car vehicles below shows driver view, front bumper view and rear view as scene, depth ans surface normals respectively. ``` From 312cbaedecb02b999e1791afb4e1fad703a2f8eb Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 9 Apr 2018 20:59:19 -0700 Subject: [PATCH 060/121] Minor spell correction --- docs/settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/settings.md b/docs/settings.md index 9c2dfe25ad..910980646a 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -206,7 +206,7 @@ This specifies the latitude, longitude and altitude of the coordinates (0, 0, 0) To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. ### PawnPaths -This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your pwn Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"/Game/MyCar/MySedanBP"}`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then chosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. +This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"/Game/MyCar/MySedanBP"}`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then chosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. ### SubWindows This setting determines what is shown in each of 3 subwindows which are visible when you press 0 key. The WindowsID can be 0 to 2, CameraID is integer identifying camera number on the vehicle. ImageType integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). For example, for car vehicles below shows driver view, front bumper view and rear view as scene, depth ans surface normals respectively. From 484ff958ada92da00b8ad6cdd7e471f1495beed7 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 9 Apr 2018 21:03:34 -0700 Subject: [PATCH 061/121] Fixed indentation --- docs/settings.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/settings.md b/docs/settings.md index 910980646a..ceff5425e8 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -42,8 +42,8 @@ Below are complete list of settings available along with their default values. I "RecordOnMove": false, "RecordInterval": 0.05, "Cameras": [ - { "CameraID": 0, "ImageType": 0, "PixelsAsFloat": false, "Compress": true } - ] + { "CameraID": 0, "ImageType": 0, "PixelsAsFloat": false, "Compress": true } + ] }, "CaptureSettings": [ { @@ -126,8 +126,8 @@ Below are complete list of settings available along with their default values. I { "X": 0.00, "Y": 0.5, "Z": 0.0, "Roll": 0.0, "Pitch": 0.0, "Yaw": 90.0 } ], "PawnPaths": { - "DefaultCar": {"PawnBP":"/AirSim/VehicleAdv/SUV/SuvCarPawn"}, - "DefaultQuadrotor": {"PawnBP":"/AirSim/Blueprints/BP_FlyingPawn"} + "DefaultCar": {"PawnBP":"/AirSim/VehicleAdv/SUV/SuvCarPawn"}, + "DefaultQuadrotor": {"PawnBP":"/AirSim/Blueprints/BP_FlyingPawn"} }, "PX4": { "FirmwareName": "PX4", From 35f25b9d941257b8562f85090a9774628750c7b9 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 10 Apr 2018 17:21:35 -0700 Subject: [PATCH 062/121] Update PX4 documentation. --- docs/px4_setup.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/docs/px4_setup.md b/docs/px4_setup.md index df9d4de36e..4c21c81c74 100644 --- a/docs/px4_setup.md +++ b/docs/px4_setup.md @@ -13,6 +13,7 @@ The following pixhawk hardware has been tested with AirSim: 2. [Pixhawk PX4 2.4.8](http://www.banggood.com/Pixhawk-PX4-2_4_8-Flight-Controller-32-Bit-ARM-PX4FMU-PX4IO-Combo-for-Multicopters-p-1040416.html) 3. [PixFalcon](https://hobbyking.com/en_us/pixfalcon-micro-px4-autopilot.html?___store=en_us) 4. [PixRacer](https://www.banggood.com/Pixracer-Autopilot-Xracer-V1_0-Flight-Controller-Mini-PX4-Built-in-Wifi-For-FPV-Racing-RC-Multirotor-p-1056428.html?utm_source=google&utm_medium=cpc_ods&utm_content=starr&utm_campaign=Smlrfpv-ds-FPVracer&gclid=CjwKEAjw9MrIBRCr2LPek5-h8U0SJAD3jfhtbEfqhX4Lu94kPe88Zrr62A5qVgx-wRDBuUulGzHELRoCRVTw_wcB) +5. [Pixhawk 2.1](http://www.proficnc.com/) (using PX4 Flight Stack) The 3DR Pixhawk Mini works out of the box, the others you may need to re-flash with the firmware v1.5.5. @@ -22,7 +23,8 @@ For this you will need one of the supported device listed above. For manual flig 1. Make sure your RC receiver is bound with its RC transmitter. Connect the RC trasmitter to the flight controller's RC port. Refer to your RC manual and [PX4 docs](https://docs.px4.io/en/getting_started/rc_transmitter_receiver.html) for more information. 2. Download [QGroundControl](http://qgroundcontrol.com/), launch it and connect your flight controller to the USB port. -3. Install the PX4 firmware v1.5.5 from [github](https://github.com/PX4/Firmware/releases/tag/v1.5.5). You likely have v2 device and if so use file `nuttx-px4fmu-v2-default.px4`. To check if you really have v2 device you can use [Nirsoft USBDeview](http://www.nirsoft.net/utils/usb_devices_view.html) utility. Chris Lovett also has created [initial firmware setup video](https://dev.px4.io/starting-initial-config.html) that show how to install new firmware. Note that later releases of PX4 at least up to 1.6.3 has [issue](https://github.com/PX4/Firmware/issues/7516) where vehicle doesn't takeoff after arming. +3. Use QGroundControl to flash the latest PX4 Flight Stack. +See also [initial firmware setup video](https://dev.px4.io/starting-initial-config.html). 4. In QGroundControl, configure your Pixhawk for HIL simulation by selecting the HIL Quadrocopter X airframe. After PX4 reboots, check that "HIL Quadrocopter X" is indeed selected. 5. In QGroundControl, go to Radio tab and calibrate (make sure, remote control is on and the receiver is showing the indicator for the binding). 6. Go to the Flight Mode tab and chose one of the remote control switches as "Mode Channel". Then set (for example) Stabilized and Attitude flight modes for two positions of the switch. @@ -31,12 +33,15 @@ For this you will need one of the supported device listed above. For manual flig ``` { "SettingsVersion": 1.0, - "DefaultVehicleConfig": "PX4" + "DefaultVehicleConfig": "PX4", + "SimMode": "Multirotor" } ``` After above setup you should be able to use RC to fly in the AirSim. You can usually arm the vehicle by lowering and bringing two sticks of RC together in-wards. You don't need QGroundControl after the initial setup. Typically the Stabilized (instead of Manual) mode gives better experience for beginners. +You can also control the drone from [Python](python.md). + See [Walkthrough Demo Video](https://youtu.be/HNWdYrtw3f0) and [Unreal AirSim Setup Video](https://youtu.be/1oY8Qu5maQQ) that shows you all the setup steps in this document. ## Setting up PX4 Software-in-Loop From 94b9d238f2668e939e0510ac86d1139c66d0afef Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 10 Apr 2018 18:00:04 -0700 Subject: [PATCH 063/121] dynamically load assets through BP classes so packaging works --- AirLib/include/common/AirSimSettings.hpp | 6 +- .../multirotor/api/MultirotorRpcLibServer.cpp | 6 +- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 15 +- .../Plugins/AirSim/Source/AirBlueprintLib.h | 1 + Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 211 +++++++++--------- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 45 ++-- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 7 +- .../Multirotor/SimModeWorldMultiRotor.cpp | 6 +- docs/settings.md | 2 +- 9 files changed, 149 insertions(+), 150 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 967978cfe9..d72e263f42 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -445,11 +445,11 @@ struct AirSimSettings { { pawn_paths.clear(); pawn_paths.emplace("BareboneCar", - PawnPath("/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn")); + PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'")); pawn_paths.emplace("DefaultCar", - PawnPath("/AirSim/VehicleAdv/SUV/SuvCarPawn")); + PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'")); pawn_paths.emplace("DefaultQuadrotor", - PawnPath("/AirSim/Blueprints/BP_FlyingPawn")); + PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'")); msr::airlib::Settings pawn_paths_child; diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index bee4df9ba2..61525b3337 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -90,9 +90,9 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string serv float obs_avoidance_vel, const MultirotorRpcLibAdapators::Vector3r& origin, float xy_length, float max_z, float min_z) -> bool { return getDroneApi()->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); }); - (static_cast(getServer()))-> - bind("setRCData", [&](const MultirotorRpcLibAdapators::RCData& data) -> - void { getDroneApi()->setRCData(data.to()); }); + (static_cast(getServer()))-> + bind("setRCData", [&](const MultirotorRpcLibAdapators::RCData& data) -> + void { getDroneApi()->setRCData(data.to()); }); //getters diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 6a36f898b1..4c76c18e70 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -604,10 +604,23 @@ UObject* UAirBlueprintLib::LoadObject(const std::string& name) FString str(name.c_str()); UObject *obj = StaticLoadObject(UObject::StaticClass(), nullptr, *str); if (obj == nullptr) { - std::string msg = "Failed to load asset - " + name; + std::string msg = "Failed to load asset object - " + name; FString fmsg(msg.c_str()); LogMessage(TEXT("Load: "), fmsg, LogDebugLevel::Failure); throw std::invalid_argument(msg); } return obj; +} + +UClass* UAirBlueprintLib::LoadClass(const std::string& name) +{ + FString str(name.c_str()); + UClass *cls = StaticLoadClass(UObject::StaticClass(), nullptr, *str); + if (cls == nullptr) { + std::string msg = "Failed to load asset class - " + name; + FString fmsg(msg.c_str()); + LogMessage(TEXT("Load: "), fmsg, LogDebugLevel::Failure); + throw std::invalid_argument(msg); + } + return cls; } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 917ef0b0d5..659670f7ac 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -106,6 +106,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static std::vector getPhysicsComponents(AActor* actor); static UObject* LoadObject(const std::string& name); + static UClass* LoadClass(const std::string& name); private: template diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 5a4673d97c..53207d318b 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -13,27 +13,10 @@ #include "PIPCamera.h" #include - - -// Needed for VR Headset -#if HMD_MODULE_INCLUDED -#include "IHeadMountedDisplay.h" -#endif // HMD_MODULE_INCLUDED - -const FName ACarPawn::LookUpBinding("LookUp"); -const FName ACarPawn::LookRightBinding("LookRight"); -const FName ACarPawn::EngineAudioRPM("RPM"); - #define LOCTEXT_NAMESPACE "VehiclePawn" - ACarPawn::ACarPawn() { - this->AutoPossessPlayer = EAutoReceiveInput::Player0; - //this->AutoReceiveInput = EAutoReceiveInput::Player0; - - UWheeledVehicleMovementComponent4W* Vehicle4W = CastChecked(GetVehicleMovement()); - static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); pip_camera_class_ = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; @@ -43,76 +26,16 @@ ACarPawn::ACarPawn() auto non_slippery_mat = Cast( UAirBlueprintLib::LoadObject(car_mesh_paths.non_slippery_mat)); if (slippery_mat) - SlipperyMaterial = slippery_mat; + slippery_mat_ = slippery_mat; + else + UAirBlueprintLib::LogMessageString("Failed to load Slippery physics material", "", LogDebugLevel::Failure); if (non_slippery_mat) - NonSlipperyMaterial = non_slippery_mat; - - check(Vehicle4W->WheelSetups.Num() == 4); - - // Wheels/Tyres - // Setup the wheels - Vehicle4W->WheelSetups[0].WheelClass = UCarWheelFront::StaticClass(); - Vehicle4W->WheelSetups[0].BoneName = FName("PhysWheel_FL"); - Vehicle4W->WheelSetups[0].AdditionalOffset = FVector(0.f, -8.f, 0.f); - - Vehicle4W->WheelSetups[1].WheelClass = UCarWheelFront::StaticClass(); - Vehicle4W->WheelSetups[1].BoneName = FName("PhysWheel_FR"); - Vehicle4W->WheelSetups[1].AdditionalOffset = FVector(0.f, 8.f, 0.f); - - Vehicle4W->WheelSetups[2].WheelClass = UCarWheelRear::StaticClass(); - Vehicle4W->WheelSetups[2].BoneName = FName("PhysWheel_BL"); - Vehicle4W->WheelSetups[2].AdditionalOffset = FVector(0.f, -8.f, 0.f); - - Vehicle4W->WheelSetups[3].WheelClass = UCarWheelRear::StaticClass(); - Vehicle4W->WheelSetups[3].BoneName = FName("PhysWheel_BR"); - Vehicle4W->WheelSetups[3].AdditionalOffset = FVector(0.f, 8.f, 0.f); - - // Adjust the tire loading - Vehicle4W->MinNormalizedTireLoad = 0.0f; - Vehicle4W->MinNormalizedTireLoadFiltered = 0.2308f; - Vehicle4W->MaxNormalizedTireLoad = 2.0f; - Vehicle4W->MaxNormalizedTireLoadFiltered = 2.0f; - - // Engine - // Torque setup - Vehicle4W->EngineSetup.MaxRPM = 5700.0f; - Vehicle4W->EngineSetup.TorqueCurve.GetRichCurve()->Reset(); - Vehicle4W->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(0.0f, 400.0f); - Vehicle4W->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(1890.0f, 500.0f); - Vehicle4W->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(5730.0f, 400.0f); - - // Adjust the steering - Vehicle4W->SteeringCurve.GetRichCurve()->Reset(); - Vehicle4W->SteeringCurve.GetRichCurve()->AddKey(0.0f, 1.0f); - Vehicle4W->SteeringCurve.GetRichCurve()->AddKey(40.0f, 0.7f); - Vehicle4W->SteeringCurve.GetRichCurve()->AddKey(120.0f, 0.6f); - - // Transmission - // We want 4wd - Vehicle4W->DifferentialSetup.DifferentialType = EVehicleDifferential4W::LimitedSlip_4W; - - // Drive the front wheels a little more than the rear - Vehicle4W->DifferentialSetup.FrontRearSplit = 0.65; - - // Automatic gearbox - Vehicle4W->TransmissionSetup.bUseGearAutoBox = true; - Vehicle4W->TransmissionSetup.GearSwitchTime = 0.15f; - Vehicle4W->TransmissionSetup.GearAutoBoxLatency = 1.0f; - - // Disable reverse as brake, this is needed for SetBreakInput() to take effect - Vehicle4W->bReverseAsBrake = false; + non_slippery_mat_ = non_slippery_mat; + else + UAirBlueprintLib::LogMessageString("Failed to load NonSlippery physics material", "", LogDebugLevel::Failure); - // Physics settings - // Adjust the center of mass - the buggy is quite low - UPrimitiveComponent* UpdatedPrimitive = Cast(Vehicle4W->UpdatedComponent); - if (UpdatedPrimitive) - { - UpdatedPrimitive->BodyInstance.COMNudge = FVector(8.0f, 0.0f, 0.0f); - } + setupVehicleMovementComponent(); - // Set the inertia scale. This controls how the mass of the vehicle is distributed. - Vehicle4W->InertiaTensorScale = FVector(1.0f, 1.333f, 1.2f); - Vehicle4W->bDeprecatedSpringOffsetMode = true; // Create In-Car camera component InternalCameraBase1 = CreateDefaultSubobject(TEXT("InternalCameraBase1")); @@ -159,11 +82,83 @@ ACarPawn::ACarPawn() GearDisplayReverseColor = FColor(255, 0, 0, 255); GearDisplayColor = FColor(255, 255, 255, 255); - bIsLowFriction = false; + is_low_friction_ = false; wrapper_.reset(new VehiclePawnWrapper()); } +void ACarPawn::setupVehicleMovementComponent() +{ + UWheeledVehicleMovementComponent4W* movement = CastChecked(GetVehicleMovement()); + check(movement->WheelSetups.Num() == 4); + + // Wheels/Tyres + // Setup the wheels + movement->WheelSetups[0].WheelClass = UCarWheelFront::StaticClass(); + movement->WheelSetups[0].BoneName = FName("PhysWheel_FL"); + movement->WheelSetups[0].AdditionalOffset = FVector(0.f, -8.f, 0.f); + + movement->WheelSetups[1].WheelClass = UCarWheelFront::StaticClass(); + movement->WheelSetups[1].BoneName = FName("PhysWheel_FR"); + movement->WheelSetups[1].AdditionalOffset = FVector(0.f, 8.f, 0.f); + + movement->WheelSetups[2].WheelClass = UCarWheelRear::StaticClass(); + movement->WheelSetups[2].BoneName = FName("PhysWheel_BL"); + movement->WheelSetups[2].AdditionalOffset = FVector(0.f, -8.f, 0.f); + + movement->WheelSetups[3].WheelClass = UCarWheelRear::StaticClass(); + movement->WheelSetups[3].BoneName = FName("PhysWheel_BR"); + movement->WheelSetups[3].AdditionalOffset = FVector(0.f, 8.f, 0.f); + + // Adjust the tire loading + movement->MinNormalizedTireLoad = 0.0f; + movement->MinNormalizedTireLoadFiltered = 0.2308f; + movement->MaxNormalizedTireLoad = 2.0f; + movement->MaxNormalizedTireLoadFiltered = 2.0f; + + // Engine + // Torque setup + movement->EngineSetup.MaxRPM = 5700.0f; + movement->EngineSetup.TorqueCurve.GetRichCurve()->Reset(); + movement->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(0.0f, 400.0f); + movement->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(1890.0f, 500.0f); + movement->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(5730.0f, 400.0f); + + // Adjust the steering + movement->SteeringCurve.GetRichCurve()->Reset(); + movement->SteeringCurve.GetRichCurve()->AddKey(0.0f, 1.0f); + movement->SteeringCurve.GetRichCurve()->AddKey(40.0f, 0.7f); + movement->SteeringCurve.GetRichCurve()->AddKey(120.0f, 0.6f); + + // Transmission + // We want 4wd + movement->DifferentialSetup.DifferentialType = EVehicleDifferential4W::LimitedSlip_4W; + + // Drive the front wheels a little more than the rear + movement->DifferentialSetup.FrontRearSplit = 0.65; + + // Automatic gearbox + movement->TransmissionSetup.bUseGearAutoBox = true; + movement->TransmissionSetup.GearSwitchTime = 0.15f; + movement->TransmissionSetup.GearAutoBoxLatency = 1.0f; + + // Disable reverse as brake, this is needed for SetBreakInput() to take effect + movement->bReverseAsBrake = false; + + // Physics settings + // Adjust the center of mass - the buggy is quite low + UPrimitiveComponent* primitive = Cast(movement->UpdatedComponent); + if (primitive) + { + primitive->BodyInstance.COMNudge = FVector(8.0f, 0.0f, 0.0f); + } + + // Set the inertia scale. This controls how the mass of the vehicle is distributed. + movement->InertiaTensorScale = FVector(1.0f, 1.333f, 1.2f); + movement->bDeprecatedSpringOffsetMode = true; +} + + void ACarPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) { @@ -299,8 +294,8 @@ void ACarPawn::setupInputBindings() UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveRight", EKeys::Left, -0.5), this, this, &ACarPawn::MoveRight); - UAirBlueprintLib::BindActionToKey("Handbrake", EKeys::End, this, &ACarPawn::OnHandbrakePressed, true); - UAirBlueprintLib::BindActionToKey("Handbrake", EKeys::End, this, &ACarPawn::OnHandbrakeReleased, false); + UAirBlueprintLib::BindActionToKey("Handbrake", EKeys::End, this, &ACarPawn::onHandbrakePressed, true); + UAirBlueprintLib::BindActionToKey("Handbrake", EKeys::End, this, &ACarPawn::onHandbrakeReleased, false); UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("Footbrake", EKeys::SpaceBar, 1), this, this, &ACarPawn::FootBrake); @@ -315,16 +310,16 @@ void ACarPawn::setupInputBindings() this, &ACarPawn::FootBrake); //below is not needed - //UAirBlueprintLib::BindActionToKey("Reverse", EKeys::Down, this, &ACarPawn::OnReversePressed, true); - //UAirBlueprintLib::BindActionToKey("Reverse", EKeys::Down, this, &ACarPawn::OnReverseReleased, false); + //UAirBlueprintLib::BindActionToKey("Reverse", EKeys::Down, this, &ACarPawn::onReversePressed, true); + //UAirBlueprintLib::BindActionToKey("Reverse", EKeys::Down, this, &ACarPawn::onReverseReleased, false); } void ACarPawn::MoveForward(float Val) { if (Val < 0) - OnReversePressed(); + onReversePressed(); else - OnReverseReleased(); + onReverseReleased(); keyboard_controls_.throttle = Val; } @@ -334,12 +329,12 @@ void ACarPawn::MoveRight(float Val) keyboard_controls_.steering = Val; } -void ACarPawn::OnHandbrakePressed() +void ACarPawn::onHandbrakePressed() { keyboard_controls_.handbrake = true; } -void ACarPawn::OnHandbrakeReleased() +void ACarPawn::onHandbrakeReleased() { keyboard_controls_.handbrake = false; } @@ -349,7 +344,7 @@ void ACarPawn::FootBrake(float Val) keyboard_controls_.brake = Val; } -void ACarPawn::OnReversePressed() +void ACarPawn::onReversePressed() { if (keyboard_controls_.manual_gear >= 0) { keyboard_controls_.is_manual_gear = true; @@ -358,7 +353,7 @@ void ACarPawn::OnReversePressed() } } -void ACarPawn::OnReverseReleased() +void ACarPawn::onReverseReleased() { if (keyboard_controls_.manual_gear < 0) { keyboard_controls_.is_manual_gear = false; @@ -391,18 +386,18 @@ void ACarPawn::Tick(float Delta) updateKinematics(Delta); - // Update phsyics material - UpdatePhysicsMaterial(); + // update phsyics material + updatePhysicsMaterial(); // Update the strings used in the hud (incar and onscreen) - UpdateHUDStrings(); + updateHUDStrings(); // Set the string in the incar hud - UpdateInCarHUD(); + updateInCarHUD(); // Pass the engine RPM to the sound component float RPMToAudioScale = 2500.0f / GetVehicleMovement()->GetEngineMaxRotationSpeed(); - EngineSoundComponent->SetFloatParameter(EngineAudioRPM, GetVehicleMovement()->GetEngineRotationSpeed()*RPMToAudioScale); + EngineSoundComponent->SetFloatParameter(FName("RPM"), GetVehicleMovement()->GetEngineRotationSpeed()*RPMToAudioScale); getVehiclePawnWrapper()->setLogLine(getLogString()); } @@ -465,7 +460,7 @@ void ACarPawn::BeginPlay() EngineSoundComponent->Play(); } -void ACarPawn::UpdateHUDStrings() +void ACarPawn::updateHUDStrings() { float vel = FMath::Abs(GetVehicleMovement()->GetForwardSpeed() / 100); //cm/s -> m/s float vel_rounded = FMath::FloorToInt(vel * 10) / 10.0f; @@ -492,7 +487,7 @@ void ACarPawn::UpdateHUDStrings() } -void ACarPawn::UpdateInCarHUD() +void ACarPawn::updateInCarHUD() { APlayerController* PlayerController = Cast(GetController()); if ((PlayerController != nullptr) && (InCarSpeed != nullptr) && (InCarGear != nullptr)) @@ -512,19 +507,19 @@ void ACarPawn::UpdateInCarHUD() } } -void ACarPawn::UpdatePhysicsMaterial() +void ACarPawn::updatePhysicsMaterial() { if (GetActorUpVector().Z < 0) { - if (bIsLowFriction == true) + if (is_low_friction_ == true) { - GetMesh()->SetPhysMaterialOverride(NonSlipperyMaterial); - bIsLowFriction = false; + GetMesh()->SetPhysMaterialOverride(non_slippery_mat_); + is_low_friction_ = false; } else { - GetMesh()->SetPhysMaterialOverride(SlipperyMaterial); - bIsLowFriction = true; + GetMesh()->SetPhysMaterialOverride(slippery_mat_); + is_low_friction_ = true; } } } diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 3c12734269..b995e526f1 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -104,30 +104,26 @@ class ACarPawn : public AWheeledVehicle /** Handle pressing right */ void MoveRight(float Val); /** Handle handbrake pressed */ - void OnHandbrakePressed(); + void onHandbrakePressed(); /** Handle handbrake released */ - void OnHandbrakeReleased(); + void onHandbrakeReleased(); /** Handle pressiong footbrake */ void FootBrake(float Val); /** Handle Reverse pressed */ - void OnReversePressed(); + void onReversePressed(); /** Handle Reverse released */ - void OnReverseReleased(); + void onReverseReleased(); /** Handle Handbrake pressed */ /** Setup the strings used on the hud */ - void UpdateInCarHUD(); + void updateInCarHUD(); - /** Update the physics material used by the vehicle mesh */ - void UpdatePhysicsMaterial(); - - static const FName LookUpBinding; - static const FName LookRightBinding; - static const FName EngineAudioRPM; + /** update the physics material used by the vehicle mesh */ + void updatePhysicsMaterial(); private: - /** Update the gear and speed strings */ - void UpdateHUDStrings(); + /** update the gear and speed strings */ + void updateHUDStrings(); void startApiServer(bool enable_rpc, const std::string& api_server_address); void stopApiServer(); bool isApiServerStarted(); @@ -135,21 +131,8 @@ class ACarPawn : public AWheeledVehicle void updateCarControls(); std::string getLogString(); + void setupVehicleMovementComponent(); - /* Are we on a 'slippery' surface */ - bool bIsLowFriction; - /** Slippery Material instance */ - UPhysicalMaterial* SlipperyMaterial; - /** Non Slippery Material instance */ - UPhysicalMaterial* NonSlipperyMaterial; - -public: - /** Returns InCarSpeed subobject **/ - FORCEINLINE UTextRenderComponent* GetInCarSpeed() const { return InCarSpeed; } - /** Returns InCarGear subobject **/ - FORCEINLINE UTextRenderComponent* GetInCarGear() const { return InCarGear; } - /** Returns EngineSoundComponent subobject **/ - FORCEINLINE UAudioComponent* GetEngineSoundComponent() const { return EngineSoundComponent; } private: typedef msr::airlib::AirSimSettings AirSimSettings; @@ -167,4 +150,12 @@ class ACarPawn : public AWheeledVehicle SimJoyStick joystick_; SimJoyStick::State joystick_state_; + + + /* Are we on a 'slippery' surface */ + bool is_low_friction_; + /** Slippery Material instance */ + UPhysicalMaterial* slippery_mat_; + /** Non Slippery Material instance */ + UPhysicalMaterial* non_slippery_mat_; }; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index d17507b9d5..c4c37377a8 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -64,12 +64,11 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - auto vehicle_bp = Cast(UAirBlueprintLib::LoadObject( - getSettings().pawn_paths.at("DefaultCar").pawn_bp)); + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( + getSettings().pawn_paths.at("DefaultCar").pawn_bp); TVehiclePawn* spawned_pawn = this->GetWorld()->SpawnActor( - vehicle_bp->GeneratedClass, - actor_transform, pawn_spawn_params); + vehicle_bp_class, actor_transform, pawn_spawn_params); spawned_actors_.Add(spawned_pawn); pawns.Add(spawned_pawn); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index 9f06927099..c0eb647623 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -99,15 +99,15 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve //if no vehicle pawns exists in environment if (pawns.Num() == 0) { - auto vehicle_bp = Cast(UAirBlueprintLib::LoadObject( - getSettings().pawn_paths.at("DefaultQuadrotor").pawn_bp)); + auto vehicle_bp_class = UAirBlueprintLib::LoadClass( + getSettings().pawn_paths.at("DefaultQuadrotor").pawn_bp); //create vehicle pawn FActorSpawnParameters pawn_spawn_params; pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; TMultiRotorPawn* spawned_pawn = this->GetWorld()->SpawnActor( - vehicle_bp->GeneratedClass, actor_transform, pawn_spawn_params); + vehicle_bp_class, actor_transform, pawn_spawn_params); spawned_actors_.Add(spawned_pawn); pawns.Add(spawned_pawn); diff --git a/docs/settings.md b/docs/settings.md index ceff5425e8..0d331b46fc 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -206,7 +206,7 @@ This specifies the latitude, longitude and altitude of the coordinates (0, 0, 0) To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. ### PawnPaths -This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"/Game/MyCar/MySedanBP"}`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then chosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. +This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then chosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. ### SubWindows This setting determines what is shown in each of 3 subwindows which are visible when you press 0 key. The WindowsID can be 0 to 2, CameraID is integer identifying camera number on the vehicle. ImageType integer value determines what kind of image gets shown according to [ImageType enum](image_apis.md#available-imagetype). For example, for car vehicles below shows driver view, front bumper view and rear view as scene, depth ans surface normals respectively. From e1e51db6a9edc138a5c7f3ba3a7b4aa0921f00c4 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 10 Apr 2018 22:39:57 -0700 Subject: [PATCH 064/121] Fix yaw mode default, remove unnecessory logging --- .../multirotor/api/MultirotorRpcLibAdapators.hpp | 4 ++-- .../simple_flight/SimpleFlightDroneController.hpp | 10 +++++----- PythonClient/survey.py | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp index 682b054976..3369b386b3 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp @@ -19,8 +19,8 @@ namespace msr { namespace airlib_rpclib { class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase { public: struct YawMode { - bool is_rate; - float yaw_or_rate; + bool is_rate = true; + float yaw_or_rate = 0; MSGPACK_DEFINE_MAP(is_rate, yaw_or_rate); YawMode() diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index b98c7a91cf..731082d239 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -227,7 +227,7 @@ class SimpleFlightDroneController : public DroneControllerBase { protected: virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override { - Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); + //Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleRate, GoalModeType::Passthrough); @@ -240,7 +240,7 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) override { - Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); + //Utils::log(Utils::stringf("commandRollPitchZ %f, %f, %f, %f", pitch, roll, z, yaw)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld); @@ -253,7 +253,7 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override { - Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); + //Utils::log(Utils::stringf("commandVelocity %f, %f, %f, %f", vx, vy, vz, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, @@ -268,7 +268,7 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override { - Utils::log(Utils::stringf("commandVelocityZ %f, %f, %f, %f", vx, vy, z, yaw_mode.yaw_or_rate)); + //Utils::log(Utils::stringf("commandVelocityZ %f, %f, %f, %f", vx, vy, z, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, @@ -283,7 +283,7 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override { - Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); + //Utils::log(Utils::stringf("commandPosition %f, %f, %f, %f", x, y, z, yaw_mode.yaw_or_rate)); typedef simple_flight::GoalModeType GoalModeType; simple_flight::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, diff --git a/PythonClient/survey.py b/PythonClient/survey.py index cd8a5514e3..517a8039c8 100644 --- a/PythonClient/survey.py +++ b/PythonClient/survey.py @@ -56,7 +56,7 @@ def start(self): trip_time = distance / self.velocity print("estimated survey time is " + str(trip_time)) try: - result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, YawMode(False,0), self.velocity + (self.velocity/2), 1) + result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, YawMode(True,0), self.velocity + (self.velocity/2), 1) except: errorType, value, traceback = sys.exc_info() print("moveOnPath threw exception: " + str(value)) From 96884d27a9291d1e341adf5ef86703a05f74c7cc Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 11 Apr 2018 12:23:50 -0700 Subject: [PATCH 065/121] PID tuning for velocity controller (removes wobble at high velocity) --- .../multirotor/firmwares/simple_flight/firmware/Params.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp index 5194f41fdf..72275bc717 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp @@ -81,8 +81,7 @@ struct Params { const float kMaxLimit = 6.0f; // m/s Axis4r max_limit = Axis4r(kMaxLimit, kMaxLimit, 0, kMaxLimit); //x, y, yaw, z in meters - const float kP = 0.5f; - Axis4r p = Axis4r(0.5f, 0.5f, 0, 2.0f); + Axis4r p = Axis4r(0.2f, 0.2f, 0, 2.0f); Axis4r i = Axis4r(0, 0, 0, 2.0f); Axis4r iterm_discount = Axis4r(1, 1, 1, 0.9999f); From 2c96616291163fcd43d0ebba4a221afc6370a6ef Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 11 Apr 2018 18:26:06 -0700 Subject: [PATCH 066/121] remove c++17 feature, fix https://github.com/Microsoft/AirSim/issues/950 --- AirLib/include/common/AirSimSettings.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index d72e263f42..a4915b9d25 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -460,9 +460,7 @@ struct AirSimSettings { for (const auto& key : keys) { msr::airlib::Settings child; pawn_paths_child.getChild(key, child); - - pawn_paths.insert_or_assign(key, - createPathPawn(child)); + pawn_paths[key] = createPathPawn(child); } } } From 881a3f8e03a1df6af886dc336cc3d2372833a11b Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 11 Apr 2018 21:15:42 -0700 Subject: [PATCH 067/121] Fix tires comming off, https://github.com/Microsoft/AirSim/issues/940 --- build.cmd | 6 +++--- setup.sh | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/build.cmd b/build.cmd index 21d3d4533d..18e436ea6c 100644 --- a/build.cmd +++ b/build.cmd @@ -75,7 +75,7 @@ robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Rel REM //---------- get High PolyCount SUV Car Model ------------ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv mkdir Unreal\Plugins\AirSim\Content\VehicleAdv -IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.9 ( +IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.10 ( IF NOT DEFINED noFullPolyCar ( REM //leave some blank lines because powershell shows download banner at top of console ECHO( @@ -89,8 +89,8 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.9 ( IF EXIST suv_download_tmp rmdir suv_download_tmp /q /s mkdir suv_download_tmp @echo on - REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" - REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" + REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" + REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" @echo off rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV diff --git a/setup.sh b/setup.sh index 29247c601d..3b82f0dba8 100755 --- a/setup.sh +++ b/setup.sh @@ -86,7 +86,7 @@ fi if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv" ]; then mkdir -p "Unreal/Plugins/AirSim/Content/VehicleAdv" fi -if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.9" ]; then +if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.10" ]; then if $downloadHighPolySuv; then echo "*********************************************************************************************" echo "Downloading high-poly car assets.... The download is ~37MB and can take some time." From 74c8d8b0003ea6ee0f0dcf88879c91f5aff7827e Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 11 Apr 2018 22:36:04 -0700 Subject: [PATCH 068/121] Sync up car_assets.zip version --- build.cmd | 2 +- setup.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build.cmd b/build.cmd index 18e436ea6c..3fbc798d1b 100644 --- a/build.cmd +++ b/build.cmd @@ -91,7 +91,7 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.1.10 ( @echo on REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" @echo off rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV powershell -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }" diff --git a/setup.sh b/setup.sh index 3b82f0dba8..06d855fc1b 100755 --- a/setup.sh +++ b/setup.sh @@ -98,7 +98,7 @@ if [ ! -d "Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/v1.1.10" ]; then fi mkdir -p "suv_download_tmp" cd suv_download_tmp - wget https://github.com/Microsoft/AirSim/releases/download/v1.1.9/car_assets.zip + wget https://github.com/Microsoft/AirSim/releases/download/v1.1.10/car_assets.zip if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" fi From 6c3154b1d57cd22f82f111073c71631f54fcc747 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 12 Apr 2018 12:47:28 -0700 Subject: [PATCH 069/121] angle normaliztion bug which caused drone to rotate unnaturally --- AirLib/include/common/VectorMath.hpp | 12 ++++++------ .../simple_flight/firmware/AngleLevelController.hpp | 11 +++++++++-- .../firmware/interfaces/CommonStructs.hpp | 12 ++++++++++++ .../multirotor/controllers/DroneControllerBase.cpp | 4 ++-- PythonClient/PythonClient.pyproj | 3 ++- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 5a17572194..8590f19505 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -397,13 +397,13 @@ class VectorMathT { , 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y())); } - static RealT normalizeAngleDegrees(RealT angle) + static RealT normalizeAngle(RealT angle, RealT max_angle = static_cast(360)) { - angle = static_cast(std::fmod(angle, 360)); - if (angle > 180) - return angle - 360; - else if (angle < -180) - return angle + 360; + angle = static_cast(std::fmod(angle, max_angle)); + if (angle > max_angle/2) + return angle - max_angle; + else if (angle < -max_angle/2) + return angle + max_angle; else return angle; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp index d911a65615..9cfadaf784 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp @@ -64,8 +64,15 @@ class AngleLevelController : //get response of level PID const auto& level_goal = goal_->getGoalValue(); - pid_->setGoal(level_goal[axis_]); - pid_->setMeasured(state_estimator_->getAngles()[axis_]); + + TReal goal_angle = level_goal[axis_]; + TReal measured_angle = state_estimator_->getAngles()[axis_]; + + TReal goal_angle_normd = Axis3r::normalizeAngle(goal_angle, 2*M_PI); + TReal measured_angle_normd = Axis3r::normalizeAngle(measured_angle, 2*M_PI); + + pid_->setGoal(goal_angle_normd); + pid_->setMeasured(measured_angle_normd); pid_->update(); //use this to drive rate controller diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index b63d469046..e3d4da76a5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -70,6 +70,18 @@ class Axis3 { return 3; } + //duplicate version of function in VectorMath + static T normalizeAngle(T angle, T max_angle = static_cast(360)) + { + angle = static_cast(std::fmod(angle, max_angle)); + if (angle > max_angle/2) + return angle - max_angle; + else if (angle < -max_angle/2) + return angle + max_angle; + else + return angle; + } + private: T vals_[3]; diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index 73f0e75733..00cc402a15 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -292,7 +292,7 @@ bool DroneControllerBase::moveToZ(float z, float velocity, const YawMode& yaw_mo bool DroneControllerBase::rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action) { - YawMode yaw_mode(false, VectorMath::normalizeAngleDegrees(yaw)); + YawMode yaw_mode(false, VectorMath::normalizeAngle(yaw)); Waiter waiter(getCommandPeriod()); auto start_pos = getPosition(); bool is_yaw_reached; @@ -646,7 +646,7 @@ void DroneControllerBase::adjustYaw(const Vector3r& heading, DrivetrainType driv if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) { if (heading.norm() > getDistanceAccuracy()) { yaw_mode.yaw_or_rate = yaw_mode.yaw_or_rate + (std::atan2(heading.y(), heading.x()) * 180 / M_PIf); - yaw_mode.yaw_or_rate = VectorMath::normalizeAngleDegrees(yaw_mode.yaw_or_rate); + yaw_mode.yaw_or_rate = VectorMath::normalizeAngle(yaw_mode.yaw_or_rate); } else yaw_mode.setZeroRate(); //don't change existing yaw if heading is too small because that can generate random result diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index a710402a52..96077bbfb9 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - hello_car.py + orbit.py . @@ -52,6 +52,7 @@ Code + Code From 129df5c289e9f38456f58ec8947a30390edcf902 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 12 Apr 2018 14:45:02 -0700 Subject: [PATCH 070/121] compute min angle distance along circle --- .../firmware/AngleLevelController.hpp | 36 ++++++++++++++++--- .../firmware/interfaces/CommonStructs.hpp | 12 ------- AirLibUnitTests/main.cpp | 2 +- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp index 9cfadaf784..42d6d34c85 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp @@ -68,11 +68,10 @@ class AngleLevelController : TReal goal_angle = level_goal[axis_]; TReal measured_angle = state_estimator_->getAngles()[axis_]; - TReal goal_angle_normd = Axis3r::normalizeAngle(goal_angle, 2*M_PI); - TReal measured_angle_normd = Axis3r::normalizeAngle(measured_angle, 2*M_PI); - - pid_->setGoal(goal_angle_normd); - pid_->setMeasured(measured_angle_normd); + adjustToMinDistanceAngles(measured_angle, goal_angle); + + pid_->setGoal(goal_angle); + pid_->setMeasured(measured_angle); pid_->update(); //use this to drive rate controller @@ -83,6 +82,7 @@ class AngleLevelController : output_ = rate_controller_->getOutput(); } + virtual TReal getOutput() override { return output_; @@ -99,6 +99,32 @@ class AngleLevelController : return rate_mode_; } +private: + static void adjustToMinDistanceAngles(TReal& angle1, TReal& angle2) + { + static constexpr TReal TwoPi = 2 * M_PIf; + + //first make sure both angles are restricted from -360 to +360 + angle1 = static_cast(std::fmod(angle1, TwoPi)); + angle2 = static_cast(std::fmod(angle2, TwoPi)); + + //now make sure both angles are restricted from 0 to 360 + if (angle1 < 0) + angle1 = TwoPi + angle1; + if (angle2 < 0) + angle2 = TwoPi + angle2; + + //measure distance between two angles + auto dist = angle1 - angle2; + + //if its > 180 then invert first angle + if (dist > M_PIf) + angle1 = angle1 - TwoPi; + //if two much on other side then invert second angle + else if (dist < -M_PIf) + angle2 = angle2 - TwoPi; + } + private: unsigned int axis_; const IGoal* goal_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index e3d4da76a5..b63d469046 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -70,18 +70,6 @@ class Axis3 { return 3; } - //duplicate version of function in VectorMath - static T normalizeAngle(T angle, T max_angle = static_cast(360)) - { - angle = static_cast(std::fmod(angle, max_angle)); - if (angle > max_angle/2) - return angle - max_angle; - else if (angle < -max_angle/2) - return angle + max_angle; - else - return angle; - } - private: T vals_[3]; diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 5c31bdc85f..5fb0927c5c 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -12,13 +12,13 @@ int main() using namespace msr::airlib; std::unique_ptr tests[] = { + std::unique_ptr(new QuaternionTest()), std::unique_ptr(new CelestialTest()), std::unique_ptr(new SettingsTest()), std::unique_ptr(new SimpleFlightTest()) //, //std::unique_ptr(new PixhawkTest()), //std::unique_ptr(new RosFlightTest()), - //std::unique_ptr(new QuaternionTest()), //std::unique_ptr(new WorkerThreadTest()) }; From 0e8aec27bf222ed4edb801e867351afe25699d14 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 12 Apr 2018 17:17:17 -0700 Subject: [PATCH 071/121] New approach for ClockSpeed for drones --- AirLib/include/common/ClockBase.hpp | 19 +++++++++ AirLib/include/common/ScalableClock.hpp | 9 ++++ AirLib/include/common/SteppableClock.hpp | 10 ++++- PythonClient/PythonClient.pyproj | 2 +- PythonClient/orbit.py | 2 +- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 11 +++++ Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 3 ++ .../Multirotor/SimModeWorldMultiRotor.cpp | 40 ++++++++++++++++++ .../Multirotor/SimModeWorldMultiRotor.h | 2 + .../AirSim/Source/SimMode/SimModeBase.cpp | 42 ++++++++++++++----- .../AirSim/Source/SimMode/SimModeBase.h | 8 +++- 11 files changed, 133 insertions(+), 15 deletions(-) diff --git a/AirLib/include/common/ClockBase.hpp b/AirLib/include/common/ClockBase.hpp index 20c28662ba..4a7c08a770 100644 --- a/AirLib/include/common/ClockBase.hpp +++ b/AirLib/include/common/ClockBase.hpp @@ -15,6 +15,13 @@ class ClockBase { //returns value indicating nanoseconds elapsed since some reference timepoint in history //typically nanoseconds from Unix epoch virtual TTimePoint nowNanos() const = 0; + virtual TTimePoint getStart() const = 0; + + + ClockBase() + { + wall_clock_start_ = Utils::getTimeSinceEpochNanos(); + } TTimeDelta elapsedSince(TTimePoint since) const { @@ -63,11 +70,23 @@ class ClockBase { std::this_thread::sleep_for(MinSleepDuration); } + double getTrueScaleWrtWallClock() + { + TTimeDelta wall_clock_now = Utils::getTimeSinceEpochNanos(); + TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_); + + TTimeDelta clock_now = nowNanos(); + TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart()); + + return static_cast(clock_elapsed) / wall_clock_elapsed; + } + private: template using duration = std::chrono::duration; uint64_t step_count_ = 0; + TTimePoint wall_clock_start_; }; }} //namespace diff --git a/AirLib/include/common/ScalableClock.hpp b/AirLib/include/common/ScalableClock.hpp index 7a8b66c682..d3102396ed 100644 --- a/AirLib/include/common/ScalableClock.hpp +++ b/AirLib/include/common/ScalableClock.hpp @@ -17,6 +17,8 @@ class ScalableClock : public ClockBase { : scale_(scale), latency_(latency) { offset_ = latency * (scale_ - 1); + start_ = nowNanos(); + unused(latency_); } @@ -36,6 +38,11 @@ class ScalableClock : public ClockBase { } } + virtual TTimePoint getStart() const override + { + return start_; + } + virtual void sleep_for(TTimeDelta dt) override { //for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate @@ -69,6 +76,8 @@ class ScalableClock : public ClockBase { double scale_; TTimeDelta latency_; double offset_; + TTimePoint start_; + }; }} //namespace diff --git a/AirLib/include/common/SteppableClock.hpp b/AirLib/include/common/SteppableClock.hpp index 829829d502..48096abb20 100644 --- a/AirLib/include/common/SteppableClock.hpp +++ b/AirLib/include/common/SteppableClock.hpp @@ -19,7 +19,7 @@ class SteppableClock : public ClockBase { SteppableClock(TTimeDelta step = 20E-3f, TTimePoint start = 0) : current_(start), step_(step) { - current_ = start ? start : Utils::getTimeSinceEpochNanos(); + start_ = current_ = start ? start : Utils::getTimeSinceEpochNanos(); } TTimePoint stepBy(TTimeDelta amount) @@ -46,8 +46,16 @@ class SteppableClock : public ClockBase { return current_; } + virtual TTimePoint getStart() const override + { + return start_; + } + + private: std::atomic current_; + std::atomic start_; + TTimeDelta step_; }; diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 96077bbfb9..d48133f253 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - orbit.py + hello_car.py . diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index 4d84bdd867..d25d66bfdf 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -192,7 +192,7 @@ def sign(self, s): arg_parser.add_argument("--altitude", type=float, help="altitude of orbit (in positive meters)", default=30) arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=5) arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") - arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 1)", default=1) + arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 1)", default=3) args = arg_parser.parse_args(args) nav = OrbitNavigator(args) nav.start() diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index c4c37377a8..f583f8373c 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -40,6 +40,17 @@ VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() return fpv_vehicle_pawn_wrapper_; } +void ASimModeCar::setupClockSpeed() +{ + float clock_speed = getSettings().clock_speed; + + //setup clock in PhysX + if (clock_speed != 1.0f) { + this->GetWorldSettings()->SetTimeDilation(clock_speed); + UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(clock_speed), LogDebugLevel::Informational); + } +} + void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) { //get player controller diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 60d453beab..5166b0bce8 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -33,6 +33,9 @@ class AIRSIM_API ASimModeCar : public ASimModeBase void updateReport(); int getRemoteControlID(const VehiclePawnWrapper& pawn); +protected: + virtual void setupClockSpeed() override; + private: UClass* external_camera_class_; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index c0eb647623..43acedb090 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -202,4 +202,44 @@ ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawn return std::static_pointer_cast(vehicle); } +void ASimModeWorldMultiRotor::setupClockSpeed() +{ + float clock_speed = getSettings().clock_speed; + + //setup clock in ClockFactory + std::string clock_type = getSettings().clock_type; + + if (clock_type == "ScalableClock") { + //scalable clock returns interval same as wall clock but multiplied by a scale factor + ClockFactory::get(std::make_shared(clock_speed == 1 ? 1 : 1 / clock_speed)); + } + else if (clock_type == "SteppableClock") { + //steppable clock returns interval that is a constant number irrespective of wall clock + //we can either multiply this fixed interval by scale factor to speed up/down the clock + //but that would cause vehicles like quadrotors to become unstable + //so alternative we use here is instead to scale control loop frequency. The downside is that + //depending on compute power available, we will max out control loop frequency and therefore can no longer + //get increase in clock speed + + //Approach 1: scale clock period, no longer used now due to quadrotor unstability + //ClockFactory::get(std::make_shared( + //static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + + //Approach 2: scale control loop frequency if clock is speeded up + if (clock_speed >= 1) { + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9))); //no clock_speed multiplier + + setPhysicsLoopPeriod(getPhysicsLoopPeriod() / static_cast(clock_speed)); + } + else { + //for slowing down, this don't generate instability + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + } + } + else + throw std::invalid_argument(common_utils::Utils::stringf( + "clock_type %s is not recognized", clock_type.c_str())); +} diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index 7f57ec130d..81fc33cf88 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -28,6 +28,8 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase virtual void createVehicles(std::vector& vehicles) override; VehiclePtr createVehicle(VehiclePawnWrapper* wrapper); + virtual void setupClockSpeed() override; + private: void setupVehiclesAndCamera(std::vector& vehicles); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 5884d1dda2..98dd3edf9a 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -31,7 +31,9 @@ void ASimModeBase::BeginPlay() { Super::BeginPlay(); - setupClock(); + setupPhysicsLoopPeriod(); + + setupClockSpeed(); setStencilIDs(); @@ -100,15 +102,11 @@ void ASimModeBase::setupTimeOfDay() } -void ASimModeBase::setupClock() +void ASimModeBase::setupClockSpeed() { - float clock_speed = getSettings().clock_speed; + //default setup - this should be overriden in derived modes as needed - //setup clock in PhysX - if (clock_speed != 1.0f) { - this->GetWorldSettings()->SetTimeDilation(clock_speed); - UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(clock_speed), LogDebugLevel::Informational); - } + float clock_speed = getSettings().clock_speed; //setup clock in ClockFactory std::string clock_type = getSettings().clock_type; @@ -123,7 +121,7 @@ void ASimModeBase::setupClock() "clock_type %s is not recognized", clock_type.c_str())); } -long long ASimModeBase::getPhysicsLoopPeriod() //nanoseconds +void ASimModeBase::setupPhysicsLoopPeriod() { /* 300Hz seems to be minimum for non-aggresive flights @@ -136,9 +134,19 @@ long long ASimModeBase::getPhysicsLoopPeriod() //nanoseconds */ if (getSettings().usage_scenario == kUsageScenarioComputerVision) - return 30000000LL; //30ms + physics_loop_period_ = 30000000LL; //30ms else - return 3000000LL; //3ms + physics_loop_period_ = 3000000LL; //3ms +} + +long long ASimModeBase::getPhysicsLoopPeriod() //nanoseconds +{ + return physics_loop_period_; +} + +void ASimModeBase::setPhysicsLoopPeriod(long long period) +{ + physics_loop_period_ = period; } void ASimModeBase::Tick(float DeltaSeconds) @@ -148,9 +156,21 @@ void ASimModeBase::Tick(float DeltaSeconds) advanceTimeOfDay(); + showClockStats(); + Super::Tick(DeltaSeconds); } +void ASimModeBase::showClockStats() +{ + float clock_speed = getSettings().clock_speed; + if (clock_speed != 1) { + UAirBlueprintLib::LogMessageString("ClockSpeed config, actual: ", + Utils::stringf("%f, %f", clock_speed, ClockFactory::get()->getTrueScaleWrtWallClock()), + LogDebugLevel::Informational); + } +} + void ASimModeBase::advanceTimeOfDay() { const auto& settings = getSettings(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 5d6cb3d554..70ecb66be9 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -53,6 +53,9 @@ class AIRSIM_API ASimModeBase : public AActor virtual void setupInputBindings(); virtual const AirSimSettings& getSettings() const; long long getPhysicsLoopPeriod(); + void setPhysicsLoopPeriod(long long period); + + virtual void setupClockSpeed(); protected: //settings int record_tick_count; @@ -71,10 +74,13 @@ class AIRSIM_API ASimModeBase : public AActor TTimePoint tod_sim_clock_start_; TTimePoint tod_last_update_; std::time_t tod_start_time_; + long long physics_loop_period_; private: void setStencilIDs(); void setupTimeOfDay(); - void setupClock(); void advanceTimeOfDay(); + void setupPhysicsLoopPeriod(); + void showClockStats(); + }; From 217ba84450a43e6041edd801b0a56e1aef9b794d Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Fri, 13 Apr 2018 13:10:07 -0700 Subject: [PATCH 072/121] fix vc++ build errors --- AirLib/include/common/ClockBase.hpp | 6 +++--- PythonClient/disarm.py | 1 - PythonClient/orbit.py | 8 ++++---- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/AirLib/include/common/ClockBase.hpp b/AirLib/include/common/ClockBase.hpp index 4a7c08a770..2bfe4363a5 100644 --- a/AirLib/include/common/ClockBase.hpp +++ b/AirLib/include/common/ClockBase.hpp @@ -72,13 +72,13 @@ class ClockBase { double getTrueScaleWrtWallClock() { - TTimeDelta wall_clock_now = Utils::getTimeSinceEpochNanos(); + TTimePoint wall_clock_now = (TTimePoint)Utils::getTimeSinceEpochNanos(); TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_); - TTimeDelta clock_now = nowNanos(); + TTimePoint clock_now = nowNanos(); TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart()); - return static_cast(clock_elapsed) / wall_clock_elapsed; + return static_cast(clock_elapsed) / static_cast(wall_clock_elapsed); } private: diff --git a/PythonClient/disarm.py b/PythonClient/disarm.py index a65bdb84d7..2eb9f9304e 100644 --- a/PythonClient/disarm.py +++ b/PythonClient/disarm.py @@ -1,5 +1,4 @@ from AirSimClient import * client = MultirotorClient() -client.confirmConnection() client.armDisarm(False) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index d25d66bfdf..f6f422c092 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -188,11 +188,11 @@ def sign(self, s): args = sys.argv args.pop(0) arg_parser = argparse.ArgumentParser("Orbit.py makes drone fly in a circle with camera pointed at the given center vector") - arg_parser.add_argument("--radius", type=float, help="radius of the orbit", default=30) - arg_parser.add_argument("--altitude", type=float, help="altitude of orbit (in positive meters)", default=30) - arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=5) + arg_parser.add_argument("--radius", type=float, help="radius of the orbit", default=10) + arg_parser.add_argument("--altitude", type=float, help="altitude of orbit (in positive meters)", default=20) + arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=3) arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") - arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 1)", default=3) + arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3) args = arg_parser.parse_args(args) nav = OrbitNavigator(args) nav.start() From 5ab22dced973eaedfdeee2af3adab41ada31e7b0 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 13 Apr 2018 16:53:22 -0700 Subject: [PATCH 073/121] fix warning as error, issue https://github.com/Microsoft/AirSim/issues/960 --- AirLib/include/common/ClockBase.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/common/ClockBase.hpp b/AirLib/include/common/ClockBase.hpp index 2bfe4363a5..d4e58c75c9 100644 --- a/AirLib/include/common/ClockBase.hpp +++ b/AirLib/include/common/ClockBase.hpp @@ -72,13 +72,13 @@ class ClockBase { double getTrueScaleWrtWallClock() { - TTimePoint wall_clock_now = (TTimePoint)Utils::getTimeSinceEpochNanos(); + TTimePoint wall_clock_now = Utils::getTimeSinceEpochNanos(); TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_); TTimePoint clock_now = nowNanos(); TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart()); - return static_cast(clock_elapsed) / static_cast(wall_clock_elapsed); + return static_cast(clock_elapsed) / wall_clock_elapsed; } private: From bdc908a11c9b790d9f8d008cc0c023cd6c14f520 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 13 Apr 2018 19:01:07 -0700 Subject: [PATCH 074/121] pause API for scheduler, better const correctness --- AirLib/include/api/VehicleApiBase.hpp | 17 ++-- .../common/common_utils/ScheduledExecutor.hpp | 27 ++++-- AirLib/include/controllers/ControllerBase.hpp | 4 +- .../controllers/VehicleControllerBase.hpp | 6 +- .../include/vehicles/car/api/CarApiBase.hpp | 2 +- .../vehicles/multirotor/api/MultirotorApi.hpp | 30 +++---- .../controllers/DroneControllerBase.hpp | 40 ++++----- .../controllers/MavLinkDroneController.hpp | 90 +++++++++---------- .../ros_flight/RosFlightDroneController.hpp | 36 ++++---- .../SimpleFlightDroneController.hpp | 36 ++++---- .../controllers/DroneControllerBase.cpp | 14 +-- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 6 +- .../Plugins/AirSim/Source/AirBlueprintLib.h | 9 +- .../Plugins/AirSim/Source/Car/CarPawnApi.cpp | 16 ++-- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h | 16 ++-- 15 files changed, 185 insertions(+), 164 deletions(-) diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 6f6dc4c02a..6eac26639d 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -12,25 +12,26 @@ namespace msr { namespace airlib { class VehicleApiBase { public: - virtual GeoPoint getHomeGeoPoint() = 0; + virtual GeoPoint getHomeGeoPoint() const = 0; virtual void enableApiControl(bool is_enabled) = 0; virtual bool isApiControlEnabled() const = 0; virtual void reset() = 0; - virtual vector simGetImages(const vector& request) = 0; - virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) = 0; + virtual vector simGetImages(const vector& request) const = 0; + virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0; virtual void simSetPose(const Pose& pose, bool ignore_collision) = 0; - virtual Pose simGetPose() = 0; + virtual Pose simGetPose() const = 0; virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0; - virtual int simGetSegmentationObjectID(const std::string& mesh_name) = 0; + virtual int simGetSegmentationObjectID(const std::string& mesh_name) const = 0; - virtual void simPrintLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; + virtual void simPrintLogMessage(const std::string& message, + const std::string& message_param = "", unsigned char severity = 0) = 0; - virtual CollisionInfo getCollisionInfo() = 0; + virtual CollisionInfo getCollisionInfo() const = 0; - virtual Pose simGetObjectPose(const std::string& object_name) = 0; + virtual Pose simGetObjectPose(const std::string& object_name) const = 0; virtual CameraInfo getCameraInfo(int camera_id) const = 0; virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0; diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp index ac5a637a71..710cc1c211 100644 --- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp +++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp @@ -31,21 +31,36 @@ class ScheduledExecutor { callback_ = callback; period_nanos_ = period_nanos; started_ = false; + paused_ = false; } void start() { started_ = true; + paused_ = false; + sleep_time_avg_ = 0; period_count_ = 0; Utils::cleanupThread(th_); th_ = std::thread(&ScheduledExecutor::executorLoop, this); } + void pause(bool is_paused) + { + paused_ = is_paused; + } + + bool isPaused() const + { + return paused_; + } + void stop() { if (started_) { started_ = false; + paused_ = false; + try { if (th_.joinable()) { th_.join(); @@ -56,12 +71,12 @@ class ScheduledExecutor { } } - bool isRunning() + bool isRunning() const { - return started_; + return started_ && !paused_; } - double getSleepTimeAvg() + double getSleepTimeAvg() const { //TODO: make this function thread safe by using atomic types //right now this is not implemented for performance and that @@ -69,7 +84,7 @@ class ScheduledExecutor { return sleep_time_avg_; } - uint64_t getPeriodCount() + uint64_t getPeriodCount() const { return period_count_; } @@ -126,7 +141,7 @@ class ScheduledExecutor { TTimeDelta since_last_call = period_start - call_end; //is this first loop? - if (period_count_ > 0) { + if (period_count_ > 0 && !paused_) { //when we are doing work, don't let other thread to cause contention std::lock_guard locker(mutex_); @@ -153,7 +168,7 @@ class ScheduledExecutor { uint64_t period_nanos_; std::thread th_; std::function callback_; - std::atomic_bool started_; + std::atomic_bool started_, paused_; double sleep_time_avg_; uint64_t period_count_; diff --git a/AirLib/include/controllers/ControllerBase.hpp b/AirLib/include/controllers/ControllerBase.hpp index d10af173b2..0c4a934d3e 100644 --- a/AirLib/include/controllers/ControllerBase.hpp +++ b/AirLib/include/controllers/ControllerBase.hpp @@ -21,8 +21,8 @@ namespace msr { namespace airlib { class ControllerBase : public UpdatableObject { public: //return 0 to 1 (corresponds to zero to full thrust) - virtual real_T getVertexControlSignal(unsigned int rotor_index) = 0; - virtual size_t getVertexCount() = 0; + virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0; + virtual size_t getVertexCount() const = 0; virtual void getStatusMessages(std::vector& messages) { diff --git a/AirLib/include/controllers/VehicleControllerBase.hpp b/AirLib/include/controllers/VehicleControllerBase.hpp index 7ba86852a4..12574b3988 100644 --- a/AirLib/include/controllers/VehicleControllerBase.hpp +++ b/AirLib/include/controllers/VehicleControllerBase.hpp @@ -19,12 +19,12 @@ class VehicleControllerBase : public ControllerBase { //tells the controller to switch from human operated mode to computer operated mode virtual void enableApiControl(bool is_enabled) = 0; virtual void setSimulationMode(bool is_set) = 0; - virtual bool isApiControlEnabled() = 0; - virtual bool isSimulationMode() = 0; + virtual bool isApiControlEnabled() const = 0; + virtual bool isSimulationMode() const = 0; //if controller connects via USB/UDP and connection fails then this //should return false - virtual bool isAvailable(std::string& message) = 0; + virtual bool isAvailable(std::string& message) const = 0; //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support. virtual void setGroundTruth(PhysicsBody* physics_body) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 6dc34c3bbd..67b254c5a6 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -62,7 +62,7 @@ class CarApiBase : public VehicleApiBase { }; virtual void setCarControls(const CarControls& controls) = 0; - virtual CarState getCarState() = 0; + virtual CarState getCarState() const = 0; virtual const CarApiBase::CarControls& getCarControls() const = 0; virtual ~CarApiBase() = default; diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index aa688eac56..829133c155 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -163,7 +163,7 @@ class MultirotorApi : public VehicleApiBase { } /************************* State APIs *********************************/ - MultirotorState getMultirotorState() + MultirotorState getMultirotorState() const { MultirotorState state; state.kinematics_estimated = controller_->getKinematicsEstimated(); @@ -175,44 +175,44 @@ class MultirotorApi : public VehicleApiBase { return state; } - Vector3r getPosition() + Vector3r getPosition() const { return controller_->getPosition(); } - Vector3r getVelocity() + Vector3r getVelocity() const { return controller_->getVelocity(); } - Quaternionr getOrientation() + Quaternionr getOrientation() const { return controller_->getOrientation(); } - DroneControllerBase::LandedState getLandedState() + DroneControllerBase::LandedState getLandedState() const { return controller_->getLandedState(); } - virtual CollisionInfo getCollisionInfo() override + virtual CollisionInfo getCollisionInfo() const override { return controller_->getCollisionInfo(); } - RCData getRCData() + RCData getRCData() const { return controller_->getRCData(); } //TODO: add GPS health, accuracy in API - GeoPoint getGpsLocation() + GeoPoint getGpsLocation() const { return controller_->getGpsLocation(); } - bool isSimulationMode() + bool isSimulationMode() const { return controller_->isSimulationMode(); } @@ -229,7 +229,7 @@ class MultirotorApi : public VehicleApiBase { /******************* VehicleApiBase implementtaion ********************/ - virtual GeoPoint getHomeGeoPoint() override + virtual GeoPoint getHomeGeoPoint() const override { return controller_->getHomeGeoPoint(); } @@ -240,14 +240,14 @@ class MultirotorApi : public VehicleApiBase { controller_->enableApiControl(is_enabled); } - virtual vector simGetImages(const vector& requests) override + virtual vector simGetImages(const vector& requests) const override { vector responses; ImageCaptureBase* image_capture = vehicle_->getImageCapture(); image_capture->getImages(requests, responses); return responses; } - virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) override + virtual vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const override { vector request = { ImageCaptureBase::ImageRequest(camera_id, image_type)}; const vector& response = simGetImages(request); @@ -262,7 +262,7 @@ class MultirotorApi : public VehicleApiBase { vehicle_->printLogMessage(message, message_param, severity); } - virtual Pose simGetObjectPose(const std::string& actor_name) override + virtual Pose simGetObjectPose(const std::string& actor_name) const override { return vehicle_->getActorPose(actor_name); } @@ -271,7 +271,7 @@ class MultirotorApi : public VehicleApiBase { { vehicle_->setPose(pose, ignore_collision); } - virtual Pose simGetPose() override + virtual Pose simGetPose() const override { return vehicle_->getPose(); } @@ -282,7 +282,7 @@ class MultirotorApi : public VehicleApiBase { return vehicle_->setSegmentationObjectID(mesh_name, object_id, is_name_regex); } - virtual int simGetSegmentationObjectID(const std::string& mesh_name) override + virtual int simGetSegmentationObjectID(const std::string& mesh_name) const override { return vehicle_->getSegmentationObjectID(mesh_name); } diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 77d94644a7..66c7a879e0 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -173,37 +173,37 @@ class DroneControllerBase : public VehicleControllerBase { virtual bool hover(CancelableBase& cancelable_action); /// get state from estimator - virtual Kinematics::State getKinematicsEstimated() = 0; + virtual Kinematics::State getKinematicsEstimated() const = 0; /// get the current local position in NED coordinate (x=North/y=East,z=Down) so z is negative. - virtual Vector3r getPosition() = 0; + virtual Vector3r getPosition() const = 0; /// Get the current velocity of the drone - virtual Vector3r getVelocity() = 0; + virtual Vector3r getVelocity() const = 0; /// Get the current orientation (or attitude) of the drone as a Quaternion. - virtual Quaternionr getOrientation() = 0; + virtual Quaternionr getOrientation() const = 0; /// Get debug pose, meaning of which is dependent on application usage. For example, /// this could be pose of real vehicle from log playback. - virtual Pose getDebugPose(); + virtual Pose getDebugPose() const; /// get the current X and Y position - Vector2r getPositionXY(); + Vector2r getPositionXY() const; /// Get the Z position (z starts at zero on the ground, and becomes more and more negative as you go up) - float getZ(); + float getZ() const; /// Get current state of the drone, is it landed or in the air - virtual LandedState getLandedState() = 0; + virtual LandedState getLandedState() const = 0; /// Assigned remote control to use for this controller, /// -1 = onboard RC, 0+ = joystick ID available on OS - virtual int getRemoteControlID() { return -1; } + virtual int getRemoteControlID() const { return -1; } /// Get the current RC inputs when RC transmitter is talking to to flight controller - virtual RCData getRCData() = 0; + virtual RCData getRCData() const = 0; virtual RCData estimateRCTrims(CancelableBase& cancelable_action, float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); @@ -212,22 +212,22 @@ class DroneControllerBase : public VehicleControllerBase { /// Get the home point (where drone was armed before takeoff). This is the location the drone /// will return to if you call goHome(). - virtual GeoPoint getHomeGeoPoint() = 0; + virtual GeoPoint getHomeGeoPoint() const = 0; /// Get the current GPS location of the drone. - virtual GeoPoint getGpsLocation() = 0; + virtual GeoPoint getGpsLocation() const = 0; //below are for passing information from simulator to API layer //in non simulation mode default would be no collision unless //controller implements otherwise. - virtual CollisionInfo getCollisionInfo(); + virtual CollisionInfo getCollisionInfo() const; virtual void setCollisionInfo(const CollisionInfo& collision_info); //safety settings virtual void setSafetyEval(const shared_ptr safety_eval_ptr); virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); - virtual const VehicleParams& getVehicleParams() = 0; + virtual const VehicleParams& getVehicleParams() const = 0; //*********************************common pre & post for move commands*************************************************** //TODO: make these protected @@ -249,16 +249,16 @@ class DroneControllerBase : public VehicleControllerBase { virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; //config commands - virtual float getCommandPeriod() = 0; //time between two command required for drone in seconds - virtual float getTakeoffZ() = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. - virtual float getDistanceAccuracy() = 0; + virtual float getDistanceAccuracy() const = 0; protected: //optional oveerides recommanded for any drones, default implementation may work virtual float getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor = 40, float min_factor = 30); - virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel); + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; protected: //utility functions and data members for derived classes typedef std::function WaitFunction; @@ -345,7 +345,7 @@ class DroneControllerBase : public VehicleControllerBase { void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); - bool isYawWithinMargin(float yaw_target, float margin); + bool isYawWithinMargin(float yaw_target, float margin) const; private:// vars shared_ptr safety_eval_ptr_; diff --git a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp index 91a4c7fb44..a1f755027a 100644 --- a/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/MavLinkDroneController.hpp @@ -95,7 +95,7 @@ class MavLinkDroneController : public DroneControllerBase //non-base interface specific to MavLinKDroneController void initialize(const ConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation); - ConnectionInfo getMavConnectionInfo(); + ConnectionInfo getMavConnectionInfo() const; static std::string findPX4(); //TODO: get rid of below methods? @@ -105,26 +105,26 @@ class MavLinkDroneController : public DroneControllerBase //*** Start: VehicleControllerBase implementation ***// virtual void reset() override; virtual void update() override; - virtual size_t getVertexCount() override; - virtual real_T getVertexControlSignal(unsigned int rotor_index) override; + virtual size_t getVertexCount() const override; + virtual real_T getVertexControlSignal(unsigned int rotor_index) const override; virtual void getStatusMessages(std::vector& messages) override; - virtual bool isAvailable(std::string& message) override; - virtual bool isApiControlEnabled() override; - virtual bool isSimulationMode() override; + virtual bool isAvailable(std::string& message) const override; + virtual bool isApiControlEnabled() const override; + virtual bool isSimulationMode() const override; virtual void enableApiControl(bool is_enabled) override; virtual void setSimulationMode(bool is_set) override; - virtual Pose getDebugPose() override; + virtual Pose getDebugPose() const override; //*** End: VehicleControllerBase implementation ***// //*** Start: DroneControllerBase implementation ***// public: - virtual Kinematics::State getKinematicsEstimated() override; - virtual Vector3r getPosition() override; - virtual Vector3r getVelocity() override; - virtual Quaternionr getOrientation() override; - virtual LandedState getLandedState() override; - virtual RCData getRCData() override; + virtual Kinematics::State getKinematicsEstimated() const override; + virtual Vector3r getPosition() const override; + virtual Vector3r getVelocity() const override; + virtual Quaternionr getOrientation() const override; + virtual LandedState getLandedState() const override; + virtual RCData getRCData() const override; virtual void setRCData(const RCData& rcData) override; virtual bool armDisarm(bool arm, CancelableBase& cancelable_action) override; @@ -132,13 +132,13 @@ class MavLinkDroneController : public DroneControllerBase virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) override; virtual bool goHome(CancelableBase& cancelable_action) override; virtual bool hover(CancelableBase& cancelable_action) override; - virtual GeoPoint getHomeGeoPoint() override; - virtual GeoPoint getGpsLocation() override; + virtual GeoPoint getHomeGeoPoint() const override; + virtual GeoPoint getGpsLocation() const override; virtual void reportTelemetry(float renderTime) override; - virtual float getCommandPeriod() override; - virtual float getTakeoffZ() override; - virtual float getDistanceAccuracy() override; + virtual float getCommandPeriod() const override; + virtual float getTakeoffZ() const override; + virtual float getDistanceAccuracy() const override; virtual bool loopCommandPre() override; virtual void loopCommandPost() override; @@ -148,7 +148,7 @@ class MavLinkDroneController : public DroneControllerBase virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override; virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override; virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override; - const VehicleParams& getVehicleParams() override; + const VehicleParams& getVehicleParams() const override; //*** End: DroneControllerBase implementation ***// private: //pimpl @@ -256,14 +256,14 @@ struct MavLinkDroneController::impl { } } - bool isAvailable(std::string& message) + bool isAvailable(std::string& message) const { if (!is_available_) message = is_available_message_; return is_available_; } - ConnectionInfo getMavConnectionInfo() + ConnectionInfo getMavConnectionInfo() const { return connection_info_; } @@ -752,23 +752,23 @@ struct MavLinkDroneController::impl { setNormalMode(); } - const ImuBase* getImu() + const ImuBase* getImu() const { return static_cast(sensors_->getByType(SensorBase::SensorType::Imu)); } - const MagnetometerBase* getMagnetometer() + const MagnetometerBase* getMagnetometer() const { return static_cast(sensors_->getByType(SensorBase::SensorType::Magnetometer)); } - const BarometerBase* getBarometer() + const BarometerBase* getBarometer() const { return static_cast(sensors_->getByType(SensorBase::SensorType::Barometer)); } - const DistanceBase* getDistance() + const DistanceBase* getDistance() const { return static_cast(sensors_->getByType(SensorBase::SensorType::Distance)); } - const GpsBase* getGps() + const GpsBase* getGps() const { return static_cast(sensors_->getByType(SensorBase::SensorType::Gps)); } @@ -1365,7 +1365,7 @@ void MavLinkDroneController::initialize(const ConnectionInfo& connection_info, c pimpl_->initialize(connection_info, sensors, is_simulation); } -MavLinkDroneController::ConnectionInfo MavLinkDroneController::getMavConnectionInfo() +MavLinkDroneController::ConnectionInfo MavLinkDroneController::getMavConnectionInfo() const { return pimpl_->getMavConnectionInfo(); } @@ -1395,11 +1395,11 @@ void MavLinkDroneController::update() DroneControllerBase::update(); pimpl_->update(); } -real_T MavLinkDroneController::getVertexControlSignal(unsigned int rotor_index) +real_T MavLinkDroneController::getVertexControlSignal(unsigned int rotor_index) const { return pimpl_->getVertexControlSignal(rotor_index); } -size_t MavLinkDroneController::getVertexCount() +size_t MavLinkDroneController::getVertexCount() const { return impl::RotorControlsCount; } @@ -1407,7 +1407,7 @@ void MavLinkDroneController::getStatusMessages(std::vector& message { pimpl_->getStatusMessages(messages); } -bool MavLinkDroneController::isAvailable(std::string& message) +bool MavLinkDroneController::isAvailable(std::string& message) const { return pimpl_->isAvailable(message); } @@ -1417,37 +1417,37 @@ bool MavLinkDroneController::isAvailable(std::string& message) //DroneControlBase -Kinematics::State MavLinkDroneController::getKinematicsEstimated() +Kinematics::State MavLinkDroneController::getKinematicsEstimated() const { return pimpl_->getKinematicsEstimated(); } -Vector3r MavLinkDroneController::getPosition() +Vector3r MavLinkDroneController::getPosition() const { return pimpl_->getPosition(); } -Vector3r MavLinkDroneController::getVelocity() +Vector3r MavLinkDroneController::getVelocity() const { return pimpl_->getVelocity(); } -Quaternionr MavLinkDroneController::getOrientation() +Quaternionr MavLinkDroneController::getOrientation() const { return pimpl_->getOrientation(); } -GeoPoint MavLinkDroneController::getHomeGeoPoint() +GeoPoint MavLinkDroneController::getHomeGeoPoint() const { return pimpl_->getHomeGeoPoint(); } -GeoPoint MavLinkDroneController::getGpsLocation() +GeoPoint MavLinkDroneController::getGpsLocation() const { return pimpl_->getGpsLocation(); } -DroneControllerBase::LandedState MavLinkDroneController::getLandedState() +DroneControllerBase::LandedState MavLinkDroneController::getLandedState() const { return pimpl_->getLandedState(); } @@ -1467,11 +1467,11 @@ void MavLinkDroneController::setSimulationMode(bool is_set) { pimpl_->setSimulationMode(is_set); } -bool MavLinkDroneController::isApiControlEnabled() +bool MavLinkDroneController::isApiControlEnabled() const { return pimpl_->isApiControlEnabled(); } -bool MavLinkDroneController::isSimulationMode() +bool MavLinkDroneController::isSimulationMode() const { return pimpl_->isSimulationMode(); } @@ -1517,7 +1517,7 @@ void MavLinkDroneController::commandPosition(float x, float y, float z, const Ya return pimpl_->commandPosition(x, y, z, yaw_mode); } -RCData MavLinkDroneController::getRCData() +RCData MavLinkDroneController::getRCData() const { return pimpl_->getRCData(); } @@ -1537,19 +1537,19 @@ void MavLinkDroneController::loopCommandPost() } //drone parameters -float MavLinkDroneController::getCommandPeriod() +float MavLinkDroneController::getCommandPeriod() const { return pimpl_->getCommandPeriod(); } -float MavLinkDroneController::getTakeoffZ() +float MavLinkDroneController::getTakeoffZ() const { return pimpl_->getTakeoffZ(); } -float MavLinkDroneController::getDistanceAccuracy() +float MavLinkDroneController::getDistanceAccuracy() const { return pimpl_->getDistanceAccuracy(); } -const VehicleParams& MavLinkDroneController::getVehicleParams() +const VehicleParams& MavLinkDroneController::getVehicleParams() const { return pimpl_->getVehicleParams(); } @@ -1560,7 +1560,7 @@ void MavLinkDroneController::reportTelemetry(float renderTime) return pimpl_->reportTelemetry(renderTime); } -Pose MavLinkDroneController::getDebugPose() +Pose MavLinkDroneController::getDebugPose() const { return pimpl_->getDebugPose(); } diff --git a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp index 5c5a6fda58..44e708de7d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/ros_flight/RosFlightDroneController.hpp @@ -61,19 +61,19 @@ class RosFlightDroneController : public DroneControllerBase { firmware_->loop(); } - virtual size_t getVertexCount() override + virtual size_t getVertexCount() const override { return vehicle_params_->getParams().rotor_count; } - virtual bool isAvailable(std::string& message) override + virtual bool isAvailable(std::string& message) const override { unused(message); return true; } - virtual real_T getVertexControlSignal(unsigned int rotor_index) override + virtual real_T getVertexControlSignal(unsigned int rotor_index) const override { //convert counter clockwise index to ArduCopter's QuadX style index unsigned int index_quadx; @@ -97,13 +97,13 @@ class RosFlightDroneController : public DroneControllerBase { comm_link_->getStatusMessages(messages); } - virtual bool isApiControlEnabled() override + virtual bool isApiControlEnabled() const override { //TODO: support offboard mode return false; } - virtual bool isSimulationMode() override + virtual bool isSimulationMode() const override { return true; } @@ -123,38 +123,38 @@ class RosFlightDroneController : public DroneControllerBase { //*** Start: DroneControllerBase implementation ***// public: - virtual Kinematics::State getKinematicsEstimated() override + virtual Kinematics::State getKinematicsEstimated() const override { return *kinematics_; } - virtual Vector3r getPosition() override + virtual Vector3r getPosition() const override { return kinematics_->pose.position; } - virtual Vector3r getVelocity() override + virtual Vector3r getVelocity() const override { return kinematics_->twist.linear; } - virtual Quaternionr getOrientation() override + virtual Quaternionr getOrientation() const override { return kinematics_->pose.orientation; } - virtual LandedState getLandedState() override + virtual LandedState getLandedState() const override { //TODO: implement this return LandedState::Landed; } - virtual int getRemoteControlID() override + virtual int getRemoteControlID() const override { return remote_control_id_; } - virtual RCData getRCData() override + virtual RCData getRCData() const override { return RCData(); } @@ -214,12 +214,12 @@ class RosFlightDroneController : public DroneControllerBase { return true; } - virtual GeoPoint getHomeGeoPoint() override + virtual GeoPoint getHomeGeoPoint() const override { return environment_->getInitialState().geo_point; } - virtual GeoPoint getGpsLocation() override + virtual GeoPoint getGpsLocation() const override { return environment_->getState().geo_point; } @@ -231,19 +231,19 @@ class RosFlightDroneController : public DroneControllerBase { //TODO: implement this } - virtual float getCommandPeriod() override + virtual float getCommandPeriod() const override { return 1.0f/50; //50hz } - virtual float getTakeoffZ() override + virtual float getTakeoffZ() const override { // pick a number, 3 meters is probably safe // enough to get out of the backwash turbulance. Negative due to NED coordinate system. return -3.0f; } - virtual float getDistanceAccuracy() override + virtual float getDistanceAccuracy() const override { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled } @@ -299,7 +299,7 @@ class RosFlightDroneController : public DroneControllerBase { //TODO: implement this } - virtual const VehicleParams& getVehicleParams() override + virtual const VehicleParams& getVehicleParams() const override { //used for safety algos. For now just use defaults static const VehicleParams safety_params; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 731082d239..0e7e91ea2c 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -64,18 +64,18 @@ class SimpleFlightDroneController : public DroneControllerBase { firmware_->update(); } - virtual size_t getVertexCount() override + virtual size_t getVertexCount() const override { return vehicle_params_->getParams().rotor_count; } - virtual bool isAvailable(std::string& message) override + virtual bool isAvailable(std::string& message) const override { unused(message); return true; } - virtual real_T getVertexControlSignal(unsigned int rotor_index) override + virtual real_T getVertexControlSignal(unsigned int rotor_index) const override { auto control_signal = board_->getMotorControlSignal(rotor_index); return control_signal; @@ -86,12 +86,12 @@ class SimpleFlightDroneController : public DroneControllerBase { comm_link_->getStatusMessages(messages); } - virtual bool isApiControlEnabled() override + virtual bool isApiControlEnabled() const override { return firmware_->offboardApi().hasApiControl(); } - virtual bool isSimulationMode() override + virtual bool isSimulationMode() const override { //TODO: after we get real board implementation, change this return true; @@ -117,42 +117,42 @@ class SimpleFlightDroneController : public DroneControllerBase { //*** Start: DroneControllerBase implementation ***// public: - virtual Kinematics::State getKinematicsEstimated() override + virtual Kinematics::State getKinematicsEstimated() const override { return AirSimSimpleFlightCommon::toKinematicsState3r(firmware_->offboardApi(). getStateEstimator().getKinematicsEstimated()); } - virtual Vector3r getPosition() override + virtual Vector3r getPosition() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getPosition(); return AirSimSimpleFlightCommon::toVector3r(val); } - virtual Vector3r getVelocity() override + virtual Vector3r getVelocity() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getLinearVelocity(); return AirSimSimpleFlightCommon::toVector3r(val); } - virtual Quaternionr getOrientation() override + virtual Quaternionr getOrientation() const override { const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation(); return AirSimSimpleFlightCommon::toQuaternion(val); } - virtual LandedState getLandedState() override + virtual LandedState getLandedState() const override { //TODO: implement this return LandedState::Landed; } - virtual int getRemoteControlID() override + virtual int getRemoteControlID() const override { return remote_control_id_; } - virtual RCData getRCData() override + virtual RCData getRCData() const override { return last_rcData_; } @@ -191,12 +191,12 @@ class SimpleFlightDroneController : public DroneControllerBase { return firmware_->offboardApi().disarm(message); } - virtual GeoPoint getHomeGeoPoint() override + virtual GeoPoint getHomeGeoPoint() const override { return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint()); } - virtual GeoPoint getGpsLocation() override + virtual GeoPoint getGpsLocation() const override { return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint()); } @@ -207,19 +207,19 @@ class SimpleFlightDroneController : public DroneControllerBase { //TODO: implement this } - virtual float getCommandPeriod() override + virtual float getCommandPeriod() const override { return 1.0f/50; //50hz } - virtual float getTakeoffZ() override + virtual float getTakeoffZ() const override { // pick a number, 3 meters is probably safe // enough to get out of the backwash turbulance. Negative due to NED coordinate system. return params_.takeoff.takeoff_z; } - virtual float getDistanceAccuracy() override + virtual float getDistanceAccuracy() const override { return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance travelled } @@ -296,7 +296,7 @@ class SimpleFlightDroneController : public DroneControllerBase { firmware_->offboardApi().setGoalAndMode(&goal, &mode, message); } - virtual const VehicleParams& getVehicleParams() override + virtual const VehicleParams& getVehicleParams() const override { return safety_params_; } diff --git a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp index 00cc402a15..80dd94e7be 100644 --- a/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp +++ b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp @@ -15,7 +15,7 @@ namespace msr { namespace airlib { float DroneControllerBase::getAutoLookahead(float velocity, float adaptive_lookahead, - float max_factor, float min_factor) + float max_factor, float min_factor) const { //if auto mode requested for lookahead then calculate based on velocity float command_period_dist = velocity * getCommandPeriod(); @@ -24,7 +24,7 @@ float DroneControllerBase::getAutoLookahead(float velocity, float adaptive_looka return lookahead; } -float DroneControllerBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) +float DroneControllerBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const { unused(risk_dist); return max_obs_avoidance_vel; @@ -499,14 +499,14 @@ bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, return waiter.is_timeout(); } -Vector2r DroneControllerBase::getPositionXY() +Vector2r DroneControllerBase::getPositionXY() const { const Vector3r& cur_loc3 = getPosition(); Vector2r cur_loc(cur_loc3.x(), cur_loc3.y()); return cur_loc; } -float DroneControllerBase::getZ() +float DroneControllerBase::getZ() const { return getPosition().z(); } @@ -699,19 +699,19 @@ void DroneControllerBase::moveToPathPosition(const Vector3r& dest, float velocit moveByVelocity(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode); } -bool DroneControllerBase::isYawWithinMargin(float yaw_target, float margin) +bool DroneControllerBase::isYawWithinMargin(float yaw_target, float margin) const { const float yaw_current = VectorMath::getYaw(getOrientation()) * 180 / M_PIf; return std::abs(yaw_current - yaw_target) <= margin; } -Pose DroneControllerBase::getDebugPose() +Pose DroneControllerBase::getDebugPose() const { //by default indicate that we don't have alternative pose info return Pose::nanPose(); } -CollisionInfo DroneControllerBase::getCollisionInfo() +CollisionInfo DroneControllerBase::getCollisionInfo() const { return collision_info_; } diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 4c76c18e70..1f3f1f1cf8 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -456,7 +456,8 @@ bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, co return actor->GetWorld()->LineTraceTestByChannel(start, end, collision_channel, trace_params); } -bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) +bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) { hit = FHitResult(ForceInit); @@ -468,7 +469,8 @@ bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, co return actor->GetWorld()->LineTraceSingleByChannel(hit, start, end, collision_channel, trace_params); } -bool UAirBlueprintLib::GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) +bool UAirBlueprintLib::GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) { TArray hits; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 659670f7ac..2d1788b867 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -47,9 +47,12 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static T* FindActor(const UObject* context, FString name); template static void FindAllActor(const UObject* context, TArray& foundActors); - static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); - static bool GetObstacle(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); - static bool GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); + static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, + const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); + static bool GetObstacle(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); + static bool GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); static void FollowActor(AActor* follower, const AActor* followee, const FVector& offset, bool fixed_z = false, float fixed_z_val = 2.0f); static bool SetMeshStencilID(const std::string& mesh_name, int object_id, diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index de10701c8b..21f401d618 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -8,7 +8,7 @@ CarPawnApi::CarPawnApi(VehiclePawnWrapper* pawn, UWheeledVehicleMovementComponen } std::vector CarPawnApi::simGetImages( - const std::vector& requests) + const std::vector& requests) const { std::vector responses; @@ -33,17 +33,17 @@ void CarPawnApi::simPrintLogMessage(const std::string& message, const std::strin pawn_->printLogMessage(message, message_param, severity); } -int CarPawnApi::simGetSegmentationObjectID(const std::string& mesh_name) +int CarPawnApi::simGetSegmentationObjectID(const std::string& mesh_name) const { return UAirBlueprintLib::GetMeshStencilID(mesh_name); } -msr::airlib::CollisionInfo CarPawnApi::getCollisionInfo() +msr::airlib::CollisionInfo CarPawnApi::getCollisionInfo() const { return pawn_->getCollisionInfo(); } -std::vector CarPawnApi::simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) +std::vector CarPawnApi::simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const { std::vector request = { ImageCaptureBase::ImageRequest(camera_id, image_type) }; const std::vector& response = simGetImages(request); @@ -71,7 +71,7 @@ void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls) movement_->SetUseAutoGears(!controls.is_manual_gear); } -msr::airlib::Pose CarPawnApi::simGetObjectPose(const std::string& actor_name) +msr::airlib::Pose CarPawnApi::simGetObjectPose(const std::string& actor_name) const { msr::airlib::Pose pose; @@ -99,7 +99,7 @@ void CarPawnApi::setCameraOrientation(int camera_id, const msr::airlib::Quaterni }, true); } -CarApiBase::CarState CarPawnApi::getCarState() +CarApiBase::CarState CarPawnApi::getCarState() const { CarApiBase::CarState state( movement_->GetForwardSpeed() / 100, //cm/s -> m/s @@ -142,12 +142,12 @@ void CarPawnApi::simSetPose(const msr::airlib::Pose& pose, bool ignore_collision }, true); } -msr::airlib::Pose CarPawnApi::simGetPose() +msr::airlib::Pose CarPawnApi::simGetPose() const { return pawn_->getPose(); } -msr::airlib::GeoPoint CarPawnApi::getHomeGeoPoint() +msr::airlib::GeoPoint CarPawnApi::getHomeGeoPoint() const { return pawn_->getHomePoint(); } diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h index 5e2e63ce03..db5a40639d 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h @@ -20,32 +20,32 @@ class CarPawnApi : public msr::airlib::CarApiBase { virtual void simPrintLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual int simGetSegmentationObjectID(const std::string& mesh_name) override; + virtual int simGetSegmentationObjectID(const std::string& mesh_name) const override; - virtual msr::airlib::CollisionInfo getCollisionInfo() override; + virtual msr::airlib::CollisionInfo getCollisionInfo() const override; - virtual std::vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) override; + virtual std::vector simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const override; virtual std::vector simGetImages( - const std::vector& requests) override; + const std::vector& requests) const override; virtual void setCarControls(const CarApiBase::CarControls& controls) override; - virtual CarApiBase::CarState getCarState() override; + virtual CarApiBase::CarState getCarState() const override; virtual void reset() override; virtual void simSetPose(const msr::airlib::Pose& pose, bool ignore_collision) override; - virtual msr::airlib::Pose simGetPose() override; + virtual msr::airlib::Pose simGetPose() const override; - virtual msr::airlib::GeoPoint getHomeGeoPoint() override; + virtual msr::airlib::GeoPoint getHomeGeoPoint() const override; virtual void enableApiControl(bool is_enabled) override; virtual bool isApiControlEnabled() const override; virtual const CarApiBase::CarControls& getCarControls() const override; - virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name) override; + virtual msr::airlib::Pose simGetObjectPose(const std::string& actor_name) const override; virtual msr::airlib::CameraInfo getCameraInfo(int camera_id) const override; virtual void setCameraOrientation(int camera_id, const msr::airlib::Quaternionr& orientation) override; From 5f628d7aa0b753afa74475a25df78d906df5a57c Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 13 Apr 2018 19:38:32 -0700 Subject: [PATCH 075/121] Fix Clang warning as error --- .../Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp | 1 + Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h | 1 + Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h | 1 + Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h | 1 + 4 files changed, 4 insertions(+) diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index 43acedb090..fd26789598 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -3,6 +3,7 @@ #include "AirBlueprintLib.h" #include "vehicles/multirotor/controllers/DroneControllerBase.hpp" #include "physics/PhysicsBody.hpp" +#include "common/ClockFactory.hpp" #include #include "Logging/MessageLog.h" #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index 81fc33cf88..f8c4175340 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -35,6 +35,7 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase void setupVehiclesAndCamera(std::vector& vehicles); private: + typedef msr::airlib::ClockFactory ClockFactory; TArray image_; std::vector > vehicle_params_; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 70ecb66be9..08d17a2de9 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -8,6 +8,7 @@ #include "VehiclePawnWrapper.h" #include "common/AirSimSettings.hpp" #include "Components/SkyLightComponent.h" +#include "common/ClockFactory.hpp" #include "Engine/DirectionalLight.h" #include "SimModeBase.generated.h" diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index e7684b7b59..45cb424cda 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -41,6 +41,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase private: typedef msr::airlib::UpdatableObject UpdatableObject; typedef msr::airlib::PhysicsEngineBase PhysicsEngineBase; + typedef msr::airlib::ClockFactory ClockFactory; PhysicsEngineBase* createPhysicsEngine(); static std::vector toUpdatableObjects(const std::vector& vehicles); From a7caebb91894680439a24ca08bf4670895781f9a Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 13 Apr 2018 19:48:19 -0700 Subject: [PATCH 076/121] const correctness for SimMode --- Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp | 4 ++-- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 4 ++-- .../Source/Multirotor/SimModeWorldMultiRotor.cpp | 4 ++-- .../AirSim/Source/Multirotor/SimModeWorldMultiRotor.h | 4 ++-- Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp | 10 +++++----- Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h | 10 +++++----- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index f583f8373c..e5d0baf6bc 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -35,7 +35,7 @@ void ASimModeCar::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } -VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() +VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() const { return fpv_vehicle_pawn_wrapper_; } @@ -133,7 +133,7 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) } -int ASimModeCar::getRemoteControlID(const VehiclePawnWrapper& pawn) +int ASimModeCar::getRemoteControlID(const VehiclePawnWrapper& pawn) const { msr::airlib::Settings settings; fpv_vehicle_pawn_wrapper_->getRawVehicleSettings(settings); diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 5166b0bce8..62c73591ca 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -22,7 +22,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; virtual void Tick(float DeltaSeconds) override; - virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper() override; + virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper() const override; void createVehicles(std::vector& vehicles); virtual void reset() override; @@ -31,7 +31,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase private: void setupVehiclesAndCamera(std::vector& vehicles); void updateReport(); - int getRemoteControlID(const VehiclePawnWrapper& pawn); + int getRemoteControlID(const VehiclePawnWrapper& pawn) const; protected: virtual void setupClockSpeed() override; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index fd26789598..b115cd3c5e 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -52,7 +52,7 @@ void ASimModeWorldMultiRotor::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } -VehiclePawnWrapper* ASimModeWorldMultiRotor::getFpvVehiclePawnWrapper() +VehiclePawnWrapper* ASimModeWorldMultiRotor::getFpvVehiclePawnWrapper() const { return fpv_vehicle_pawn_wrapper_; } @@ -150,7 +150,7 @@ void ASimModeWorldMultiRotor::Tick(float DeltaSeconds) getFpvVehiclePawnWrapper()->setLogLine(getLogString()); } -std::string ASimModeWorldMultiRotor::getLogString() +std::string ASimModeWorldMultiRotor::getLogString() const { const msr::airlib::Kinematics::State* kinematics = getFpvVehiclePawnWrapper()->getTrueKinematics(); uint64_t timestamp_millis = static_cast(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E6); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index f8c4175340..f304e1cb80 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -20,8 +20,8 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase virtual void Tick( float DeltaSeconds ) override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; - VehiclePawnWrapper* getFpvVehiclePawnWrapper() override; - std::string getLogString(); + VehiclePawnWrapper* getFpvVehiclePawnWrapper() const override; + std::string getLogString() const; protected: typedef AFlyingPawn TMultiRotorPawn; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 98dd3edf9a..e73e773d72 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -139,7 +139,7 @@ void ASimModeBase::setupPhysicsLoopPeriod() physics_loop_period_ = 3000000LL; //3ms } -long long ASimModeBase::getPhysicsLoopPeriod() //nanoseconds +long long ASimModeBase::getPhysicsLoopPeriod() const //nanoseconds { return physics_loop_period_; } @@ -203,7 +203,7 @@ void ASimModeBase::reset() //Should be overridden by derived classes } -VehiclePawnWrapper* ASimModeBase::getFpvVehiclePawnWrapper() +VehiclePawnWrapper* ASimModeBase::getFpvVehiclePawnWrapper() const { //Should be overridden by derived classes return nullptr; @@ -224,17 +224,17 @@ void ASimModeBase::setupInputBindings() UAirBlueprintLib::BindActionToKey("InputEventResetAll", EKeys::BackSpace, this, &ASimModeBase::reset); } -bool ASimModeBase::isRecording() +bool ASimModeBase::isRecording() const { return FRecordingThread::isRecording(); } -bool ASimModeBase::isRecordUIVisible() +bool ASimModeBase::isRecordUIVisible() const { return getSettings().is_record_ui_visible; } -ECameraDirectorMode ASimModeBase::getInitialViewMode() +ECameraDirectorMode ASimModeBase::getInitialViewMode() const { return Utils::toEnum(getSettings().initial_view_mode); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 08d17a2de9..e8dfb5d0f6 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -40,20 +40,20 @@ class AIRSIM_API ASimModeBase : public AActor virtual std::string getReport(); virtual void startRecording(); virtual void stopRecording(); - virtual bool isRecording(); - virtual bool isRecordUIVisible(); - virtual ECameraDirectorMode getInitialViewMode(); + virtual bool isRecording() const; + virtual bool isRecordUIVisible() const; + virtual ECameraDirectorMode getInitialViewMode() const; //must be implemented by derived class //can't use pure virtual because of restriction with Unreal - virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper(); + virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper() const; protected: typedef msr::airlib::AirSimSettings AirSimSettings; virtual void setupInputBindings(); virtual const AirSimSettings& getSettings() const; - long long getPhysicsLoopPeriod(); + long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); virtual void setupClockSpeed(); From a66380e0330f6aeb753311c39db8aab8566cd0a5 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Mon, 16 Apr 2018 19:25:38 -0700 Subject: [PATCH 077/121] Add snapshots option to the orbit.py script. --- PythonClient/orbit.py | 46 ++++++++++++++++++++++++++++++++++++------- 1 file changed, 39 insertions(+), 7 deletions(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index f6f422c092..ee5068265c 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -17,6 +17,13 @@ def __init__(self, args): self.altitude = args.altitude self.speed = args.speed self.iterations = args.iterations + self.snapshots = args.snapshots + self.z = None + self.snapshot_index = 0 + + if self.snapshots > 0: + self.snapshot_delta = 360 / self.snapshots + if self.iterations <= 0: self.iterations = 1 @@ -77,21 +84,23 @@ def start(self): print("climbing to position: {},{},{}".format(start.x, start.y, z)) self.client.moveToPosition(start.x, start.y, z, self.speed) - + self.z = z + print("ramping up to speed...") count = 0 self.start_angle = None + self.next_snapshot = None # ramp up time ramptime = self.radius / 10 - start_time = time.time() + self.start_time = time.time() while count < self.iterations: # ramp up to full speed in smooth increments so we don't start too aggresively. now = time.time() speed = self.speed - diff = now - start_time + diff = now - self.start_time if diff < ramptime: speed = self.speed * diff / ramptime elif ramptime > 0: @@ -120,6 +129,7 @@ def start(self): count += 1 print("completed {} orbits".format(count)) + self.camera_heading = camera_heading self.client.moveByVelocityZ(vx, vy, z, 1, DrivetrainType.MaxDegreeOfFreedom, YawMode(False, camera_heading)) if z < self.home.z: @@ -140,6 +150,8 @@ def track_orbits(self, angle): if self.start_angle is None: self.start_angle = angle + if self.snapshot_delta: + self.next_snapshot = angle + self.snapshot_delta self.previous_angle = angle self.shifted = False self.previous_sign = None @@ -150,16 +162,23 @@ def track_orbits(self, angle): # now we just have to watch for a smooth crossing from negative diff to positive diff if self.previous_angle is None: self.previous_angle = angle + return False + + # ignore the click over from 360 back to 0 + if self.previous_angle > 350 and angle < 10: + if self.next_snapshot >= 360: + self.next_snapshot -= 360 return False diff = self.previous_angle - angle crossing = False self.previous_angle = angle - # ignore any large discontinuities - if abs(diff) > 45: - return False - + if self.snapshot_delta and angle > self.next_snapshot: + print("Taking snapshot at angle {}".format(angle)) + self.take_snapshot() + self.next_snapshot += self.snapshot_delta + diff = abs(angle - self.start_angle) if diff > 45: self.quarter = True @@ -179,6 +198,18 @@ def track_orbits(self, angle): return crossing + def take_snapshot(self): + # first hold our current position so drone doesn't try and keep flying while we take the picture. + pos = self.getPosition() + self.client.moveToPosition(pos.x, pos.y, self.z, 0.5, 10, DrivetrainType.MaxDegreeOfFreedom, YawMode(False, self.camera_heading)) + responses = self.client.simGetImages([ImageRequest(1, AirSimImageType.Scene)]) #scene vision image in png format + response = responses[0] + filename = "photo_" + str(self.snapshot_index) + self.snapshot_index += 1 + AirSimClientBase.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) + print("Saved snapshot: {}".format(filename)) + self.start_time = time.time() # cause smooth ramp up to happen again after photo is taken. + def sign(self, s): if s < 0: return -1 @@ -193,6 +224,7 @@ def sign(self, s): arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=3) arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3) + arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0) args = arg_parser.parse_args(args) nav = OrbitNavigator(args) nav.start() From f2bc23d5c6440444f58728138d8aa43ab8672127 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Tue, 17 Apr 2018 11:57:22 +0200 Subject: [PATCH 078/121] Thrustmaster wheel support --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 39 +++++- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 1 + .../SimJoyStick/DirectInputJoyStick.cpp | 115 +++++++++++++++++- .../Source/SimJoyStick/DirectInputJoystick.h | 5 + .../AirSim/Source/SimJoyStick/SimJoyStick.cpp | 10 ++ .../AirSim/Source/SimJoyStick/SimJoyStick.h | 24 ++-- 6 files changed, 179 insertions(+), 15 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 53207d318b..1fc84fe14d 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -384,6 +384,8 @@ void ACarPawn::Tick(float Delta) updateCarControls(); + updateForceFeedback(); + updateKinematics(Delta); // update phsyics material @@ -412,10 +414,20 @@ void ACarPawn::updateCarControls() return; } - joystick_controls_.steering = joystick_state_.left_y * 1.25; - joystick_controls_.throttle = (-joystick_state_.right_x + 1) / 2; - joystick_controls_.brake = -joystick_state_.right_z + 1; - + std::string vendorid = joystick_state_.pid_vid.substr(0, joystick_state_.pid_vid.find('&')); + + // Thrustmaster devices + if (vendorid == "VID_044F") { + joystick_controls_.steering = joystick_state_.left_x; + joystick_controls_.throttle = (-joystick_state_.right_z + 1) / 2; + joystick_controls_.brake = (joystick_state_.left_y + 1) / 2; + } + // Anything else + else { + joystick_controls_.steering = joystick_state_.left_y * 1.25; + joystick_controls_.throttle = (-joystick_state_.right_x + 1) / 2; + joystick_controls_.brake = -joystick_state_.right_z + 1; + } //Two steel levers behind wheel joystick_controls_.handbrake = (joystick_state_.buttons & 32) | (joystick_state_.buttons & 64) ? 1 : 0; @@ -452,6 +464,25 @@ void ACarPawn::updateCarControls() UAirBlueprintLib::LogMessageString("Target Gear: ", std::to_string(current_controls_.manual_gear), LogDebugLevel::Informational); } +void ACarPawn::updateForceFeedback() { + if (joystick_state_.is_initialized) { + + // Update wheel rumble + float rumblestrength = 0.66 + (GetVehicleMovement()->GetEngineRotationSpeed() + / GetVehicleMovement()->GetEngineMaxRotationSpeed()) / 3; + + joystick_.setWheelRumble(wrapper_->getRemoteControlID(), rumblestrength); + + // Update autocenter + double speed = GetVehicleMovement()->GetForwardSpeed(); + + joystick_.setAutoCenter(wrapper_->getRemoteControlID(), + ( 1.0 - 1.0 / ( std::abs(speed / 80) + 1.0)) + * (joystick_state_.left_x / 3)); + } +} + + void ACarPawn::BeginPlay() { Super::BeginPlay(); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index b995e526f1..c4e096dd90 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -129,6 +129,7 @@ class ACarPawn : public AWheeledVehicle bool isApiServerStarted(); void updateKinematics(float delta); void updateCarControls(); + void updateForceFeedback(); std::string getLogString(); void setupVehicleMovementComponent(); diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 1d03799f83..8a146582fa 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -52,6 +52,12 @@ struct DirectInputJoyStick::impl{ LPDIRECTINPUT8 g_pDI = nullptr; LPDIRECTINPUTDEVICE8 g_pJoystick = nullptr; + LPDIRECTINPUTEFFECT g_pAutoCenterHandle = nullptr; + DIEFFECT g_sAutoCenterConfig; + + LPDIRECTINPUTEFFECT g_pWheelRumbleHandle = nullptr; + DIEFFECT g_sWheelRumbleConfig; + DIJOYCONFIG PreferredJoyCfg = { 0 }; DI_ENUM_CONTEXT enumContext; @@ -65,6 +71,30 @@ struct DirectInputJoyStick::impl{ return InitDirectInput(joystick_index) == S_OK; } + // Magnitude ranges from -1 to 1 + void setAutoCenterStrength(double magnitude) { + DICONSTANTFORCE cf = { magnitude * 10000 }; + + g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); + g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; + + g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } + +#define FFWRMAX 0.08 + + // Strength ranges from 0 to 1 + void setWheelRumbleStrength(double strength) { + DIPERIODIC pf = { FFWRMAX * strength * 10000,0,0,0.06 * 1000000 }; + + g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); + g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; + + g_pWheelRumbleHandle->SetParameters(&g_sWheelRumbleConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } + const JoystickState& getState(bool update_state = true) { if (update_state) @@ -105,6 +135,83 @@ struct DirectInputJoyStick::impl{ return g; } + HRESULT InitForceFeedback() { + + HRESULT hr; + DWORD rgdwAxes[2] = { DIJOFS_X, DIJOFS_Y }; + LONG rglDirection[2] = { 0, 0 }; + + if (FAILED(hr = g_pJoystick->SetCooperativeLevel(GetActiveWindow(), DISCL_EXCLUSIVE | DISCL_BACKGROUND))) + return hr; + + if (FAILED(hr = g_pJoystick->Acquire())) + return hr; + + // Autocenter + ZeroMemory(&g_sAutoCenterConfig, sizeof(g_sAutoCenterConfig)); + + + g_sAutoCenterConfig.dwStartDelay = 0; + + g_sAutoCenterConfig.dwSize = sizeof(DIEFFECT); + g_sAutoCenterConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; + g_sAutoCenterConfig.dwDuration = INFINITE; + g_sAutoCenterConfig.dwSamplePeriod = 0; + g_sAutoCenterConfig.dwGain = DI_FFNOMINALMAX; + g_sAutoCenterConfig.dwTriggerButton = DIEB_NOTRIGGER; + g_sAutoCenterConfig.dwTriggerRepeatInterval = 0; + g_sAutoCenterConfig.cAxes = 1; + g_sAutoCenterConfig.rgdwAxes = rgdwAxes; + g_sAutoCenterConfig.rglDirection = rglDirection; + + g_sAutoCenterConfig.lpEnvelope = 0; + g_sAutoCenterConfig.dwStartDelay = 0; + + DICONSTANTFORCE cf = { 0 }; + + g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); + g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; + + if (FAILED(hr = g_pJoystick->CreateEffect(GUID_ConstantForce, &g_sAutoCenterConfig, &g_pAutoCenterHandle, nullptr))) + return hr; + + if (FAILED(hr = g_pAutoCenterHandle->Start(INFINITE, 0))) + return hr; + + // Rumble + ZeroMemory(&g_sWheelRumbleConfig, sizeof(g_sWheelRumbleConfig)); + + g_sWheelRumbleConfig.dwStartDelay = 0; + + g_sWheelRumbleConfig.dwSize = sizeof(DIEFFECT); + g_sWheelRumbleConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; + g_sWheelRumbleConfig.dwDuration = INFINITE; + g_sWheelRumbleConfig.dwSamplePeriod = 0; + g_sWheelRumbleConfig.dwGain = DI_FFNOMINALMAX; + g_sWheelRumbleConfig.dwTriggerButton = DIEB_NOTRIGGER; + g_sWheelRumbleConfig.dwTriggerRepeatInterval = 0; + g_sWheelRumbleConfig.cAxes = 1; + g_sWheelRumbleConfig.rgdwAxes = rgdwAxes; + g_sWheelRumbleConfig.rglDirection = rglDirection; + + g_sWheelRumbleConfig.lpEnvelope = 0; + g_sWheelRumbleConfig.dwStartDelay = 0; + + DIPERIODIC pf = { 0,0,0,0.08 }; + + g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); + g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; + + if (FAILED(hr = g_pJoystick->CreateEffect(GUID_Sine, &g_sWheelRumbleConfig, &g_pWheelRumbleHandle, nullptr))) + return hr; + + if (FAILED(hr = g_pWheelRumbleHandle->Start(INFINITE, 0))) + return hr; + + return S_OK; + } + + HRESULT InitDirectInput(unsigned int joystick_index) { HRESULT hr; @@ -189,13 +296,15 @@ struct DirectInputJoyStick::impl{ return hr; } + InitForceFeedback(); + state.is_initialized = true; return S_OK; } //----------------------------------------------------------------------------- // Enum each PNP device using WMI and check each device ID to see if it contains - // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it’s an XInput device + // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then itďż˝s an XInput device // Unfortunately this information can not be found by just using DirectInput. // Checking against a VID/PID of 0x028E/0x045E won't find 3rd party or future // XInput devices. @@ -270,7 +379,7 @@ struct DirectInputJoyStick::impl{ hr = pDevices[iDevice]->Get(bstrDeviceID, 0L, &var, nullptr, nullptr); if (SUCCEEDED(hr) && var.vt == VT_BSTR && var.bstrVal != nullptr) { - // Check if the device ID contains "IG_". If it does, then it’s an XInput device + // Check if the device ID contains "IG_". If it does, then itďż˝s an XInput device // Unfortunately this information can not be found by just using DirectInput if (wcsstr(var.bstrVal, L"IG_")) { @@ -558,6 +667,8 @@ struct DirectInputJoyStick::impl{ g_pJoystick->Unacquire(); // Release any DirectInput objects. + DIJT_SAFE_RELEASE(g_pAutoCenterHandle); + DIJT_SAFE_RELEASE(g_pWheelRumbleHandle); DIJT_SAFE_RELEASE(g_pJoystick); DIJT_SAFE_RELEASE(g_pDI); } diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h index d740179314..be47f7e004 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h @@ -42,6 +42,11 @@ class DirectInputJoyStick { DirectInputJoyStick(); ~DirectInputJoyStick(); bool initialize(unsigned int joystick_index); + // strength ranges from -1 to 1 + void setAutoCenter(double strength); + + // strength ranges from 0 to 1 + void setWheelRumble(double strength); const JoystickState& getState(bool update_state = true); const Capabilities& getCapabilities(); const JoystickInfo& getJoystickInfo(); diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp index d4cc0ec037..6156ada4a1 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp @@ -368,3 +368,13 @@ void SimJoyStick::getJoyStickState(unsigned int index, SimJoyStick::State& state { pimpl_->getJoyStickState(index, state, axis_maps); } + +void SimJoyStick::setAutoCenter(unsigned int index, double strength) +{ + pimpl_->setAutoCenter(index, strength); +} + +void SimJoyStick::setWheelRumble(unsigned int index, double strength) +{ + pimpl_->setWheelRumble(index, strength); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h index 4920f3a14f..60337dc7f3 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h @@ -1,11 +1,11 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. #pragma once #include #include -#include +#include class SimJoyStick { @@ -44,13 +44,19 @@ class SimJoyStick void getJoyStickState(unsigned int index, State& state); + // strength ranges from -1 to 1 + void setAutoCenter(unsigned int index, double strength); + + // strength ranges from 0 to 1 + void setWheelRumble(unsigned int index, double strength); + SimJoyStick(); - ~SimJoyStick(); //required for pimpl -private: - static bool initialized_success_; - //bool api_control_enabled_ = false; - - struct impl; + ~SimJoyStick(); //required for pimpl +private: + static bool initialized_success_; + //bool api_control_enabled_ = false; + + struct impl; std::unique_ptr pimpl_; }; From fb2ad96de714b5407b135f543756b1c011ed2031 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Tue, 17 Apr 2018 12:52:08 +0200 Subject: [PATCH 079/121] tab to space --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 2 +- .../SimJoyStick/DirectInputJoyStick.cpp | 153 +++++++++--------- .../Source/SimJoyStick/DirectInputJoystick.h | 6 +- .../AirSim/Source/SimJoyStick/SimJoyStick.cpp | 4 +- .../AirSim/Source/SimJoyStick/SimJoyStick.h | 6 +- 5 files changed, 85 insertions(+), 86 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 1fc84fe14d..5e6031653a 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -477,7 +477,7 @@ void ACarPawn::updateForceFeedback() { double speed = GetVehicleMovement()->GetForwardSpeed(); joystick_.setAutoCenter(wrapper_->getRemoteControlID(), - ( 1.0 - 1.0 / ( std::abs(speed / 80) + 1.0)) + ( 1.0 - 1.0 / ( std::abs(speed / 120) + 1.0)) * (joystick_state_.left_x / 3)); } } diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 8a146582fa..5d82536cdc 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -53,10 +53,10 @@ struct DirectInputJoyStick::impl{ LPDIRECTINPUTDEVICE8 g_pJoystick = nullptr; LPDIRECTINPUTEFFECT g_pAutoCenterHandle = nullptr; - DIEFFECT g_sAutoCenterConfig; + DIEFFECT g_sAutoCenterConfig; - LPDIRECTINPUTEFFECT g_pWheelRumbleHandle = nullptr; - DIEFFECT g_sWheelRumbleConfig; + LPDIRECTINPUTEFFECT g_pWheelRumbleHandle = nullptr; + DIEFFECT g_sWheelRumbleConfig; DIJOYCONFIG PreferredJoyCfg = { 0 }; DI_ENUM_CONTEXT enumContext; @@ -72,28 +72,28 @@ struct DirectInputJoyStick::impl{ } // Magnitude ranges from -1 to 1 - void setAutoCenterStrength(double magnitude) { - DICONSTANTFORCE cf = { magnitude * 10000 }; - - g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); - g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; + void setAutoCenterStrength(double magnitude) { + DICONSTANTFORCE cf = { magnitude * 10000 }; + + g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); + g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; - g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | - DIEP_TYPESPECIFICPARAMS | DIEP_START); - } + g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } #define FFWRMAX 0.08 - // Strength ranges from 0 to 1 - void setWheelRumbleStrength(double strength) { - DIPERIODIC pf = { FFWRMAX * strength * 10000,0,0,0.06 * 1000000 }; - - g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); - g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; + // Strength ranges from 0 to 1 + void setWheelRumbleStrength(double strength) { + DIPERIODIC pf = { FFWRMAX * strength * 10000,0,0,0.06 * 1000000 }; + + g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); + g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; - g_pWheelRumbleHandle->SetParameters(&g_sWheelRumbleConfig, DIEP_DIRECTION | - DIEP_TYPESPECIFICPARAMS | DIEP_START); - } + g_pWheelRumbleHandle->SetParameters(&g_sWheelRumbleConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } const JoystickState& getState(bool update_state = true) { @@ -137,79 +137,78 @@ struct DirectInputJoyStick::impl{ HRESULT InitForceFeedback() { - HRESULT hr; - DWORD rgdwAxes[2] = { DIJOFS_X, DIJOFS_Y }; - LONG rglDirection[2] = { 0, 0 }; - - if (FAILED(hr = g_pJoystick->SetCooperativeLevel(GetActiveWindow(), DISCL_EXCLUSIVE | DISCL_BACKGROUND))) - return hr; + HRESULT hr; + DWORD rgdwAxes[2] = { DIJOFS_X, DIJOFS_Y }; + LONG rglDirection[2] = { 0, 0 }; - if (FAILED(hr = g_pJoystick->Acquire())) - return hr; + if (FAILED(hr = g_pJoystick->SetCooperativeLevel(GetActiveWindow(), DISCL_EXCLUSIVE | DISCL_BACKGROUND))) + return hr; - // Autocenter - ZeroMemory(&g_sAutoCenterConfig, sizeof(g_sAutoCenterConfig)); + if (FAILED(hr = g_pJoystick->Acquire())) + return hr; + // Autocenter + ZeroMemory(&g_sAutoCenterConfig, sizeof(g_sAutoCenterConfig)); - g_sAutoCenterConfig.dwStartDelay = 0; + g_sAutoCenterConfig.dwStartDelay = 0; - g_sAutoCenterConfig.dwSize = sizeof(DIEFFECT); - g_sAutoCenterConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; - g_sAutoCenterConfig.dwDuration = INFINITE; - g_sAutoCenterConfig.dwSamplePeriod = 0; - g_sAutoCenterConfig.dwGain = DI_FFNOMINALMAX; - g_sAutoCenterConfig.dwTriggerButton = DIEB_NOTRIGGER; - g_sAutoCenterConfig.dwTriggerRepeatInterval = 0; - g_sAutoCenterConfig.cAxes = 1; - g_sAutoCenterConfig.rgdwAxes = rgdwAxes; - g_sAutoCenterConfig.rglDirection = rglDirection; + g_sAutoCenterConfig.dwSize = sizeof(DIEFFECT); + g_sAutoCenterConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; + g_sAutoCenterConfig.dwDuration = INFINITE; + g_sAutoCenterConfig.dwSamplePeriod = 0; + g_sAutoCenterConfig.dwGain = DI_FFNOMINALMAX; + g_sAutoCenterConfig.dwTriggerButton = DIEB_NOTRIGGER; + g_sAutoCenterConfig.dwTriggerRepeatInterval = 0; + g_sAutoCenterConfig.cAxes = 1; + g_sAutoCenterConfig.rgdwAxes = rgdwAxes; + g_sAutoCenterConfig.rglDirection = rglDirection; - g_sAutoCenterConfig.lpEnvelope = 0; - g_sAutoCenterConfig.dwStartDelay = 0; + g_sAutoCenterConfig.lpEnvelope = 0; + g_sAutoCenterConfig.dwStartDelay = 0; - DICONSTANTFORCE cf = { 0 }; + DICONSTANTFORCE cf = { 0 }; - g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); - g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; + g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); + g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; - if (FAILED(hr = g_pJoystick->CreateEffect(GUID_ConstantForce, &g_sAutoCenterConfig, &g_pAutoCenterHandle, nullptr))) - return hr; + if (FAILED(hr = g_pJoystick->CreateEffect(GUID_ConstantForce, &g_sAutoCenterConfig, &g_pAutoCenterHandle, nullptr))) + return hr; - if (FAILED(hr = g_pAutoCenterHandle->Start(INFINITE, 0))) - return hr; + if (FAILED(hr = g_pAutoCenterHandle->Start(INFINITE, 0))) + return hr; - // Rumble - ZeroMemory(&g_sWheelRumbleConfig, sizeof(g_sWheelRumbleConfig)); + // Rumble + ZeroMemory(&g_sWheelRumbleConfig, sizeof(g_sWheelRumbleConfig)); - g_sWheelRumbleConfig.dwStartDelay = 0; + g_sWheelRumbleConfig.dwStartDelay = 0; - g_sWheelRumbleConfig.dwSize = sizeof(DIEFFECT); - g_sWheelRumbleConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; - g_sWheelRumbleConfig.dwDuration = INFINITE; - g_sWheelRumbleConfig.dwSamplePeriod = 0; - g_sWheelRumbleConfig.dwGain = DI_FFNOMINALMAX; - g_sWheelRumbleConfig.dwTriggerButton = DIEB_NOTRIGGER; - g_sWheelRumbleConfig.dwTriggerRepeatInterval = 0; - g_sWheelRumbleConfig.cAxes = 1; - g_sWheelRumbleConfig.rgdwAxes = rgdwAxes; - g_sWheelRumbleConfig.rglDirection = rglDirection; + g_sWheelRumbleConfig.dwSize = sizeof(DIEFFECT); + g_sWheelRumbleConfig.dwFlags = DIEFF_CARTESIAN | DIEFF_OBJECTOFFSETS; + g_sWheelRumbleConfig.dwDuration = INFINITE; + g_sWheelRumbleConfig.dwSamplePeriod = 0; + g_sWheelRumbleConfig.dwGain = DI_FFNOMINALMAX; + g_sWheelRumbleConfig.dwTriggerButton = DIEB_NOTRIGGER; + g_sWheelRumbleConfig.dwTriggerRepeatInterval = 0; + g_sWheelRumbleConfig.cAxes = 1; + g_sWheelRumbleConfig.rgdwAxes = rgdwAxes; + g_sWheelRumbleConfig.rglDirection = rglDirection; - g_sWheelRumbleConfig.lpEnvelope = 0; - g_sWheelRumbleConfig.dwStartDelay = 0; + g_sWheelRumbleConfig.lpEnvelope = 0; + g_sWheelRumbleConfig.dwStartDelay = 0; - DIPERIODIC pf = { 0,0,0,0.08 }; + DIPERIODIC pf = { 0,0,0,0.08 }; - g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); - g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; + g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); + g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; - if (FAILED(hr = g_pJoystick->CreateEffect(GUID_Sine, &g_sWheelRumbleConfig, &g_pWheelRumbleHandle, nullptr))) - return hr; + if (FAILED(hr = g_pJoystick->CreateEffect(GUID_Sine, &g_sWheelRumbleConfig, &g_pWheelRumbleHandle, nullptr))) + return hr; - if (FAILED(hr = g_pWheelRumbleHandle->Start(INFINITE, 0))) - return hr; + if (FAILED(hr = g_pWheelRumbleHandle->Start(INFINITE, 0))) + return hr; - return S_OK; - } + return S_OK; + } HRESULT InitDirectInput(unsigned int joystick_index) @@ -304,7 +303,7 @@ struct DirectInputJoyStick::impl{ //----------------------------------------------------------------------------- // Enum each PNP device using WMI and check each device ID to see if it contains - // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then itďż˝s an XInput device + // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it's an XInput device // Unfortunately this information can not be found by just using DirectInput. // Checking against a VID/PID of 0x028E/0x045E won't find 3rd party or future // XInput devices. @@ -379,7 +378,7 @@ struct DirectInputJoyStick::impl{ hr = pDevices[iDevice]->Get(bstrDeviceID, 0L, &var, nullptr, nullptr); if (SUCCEEDED(hr) && var.vt == VT_BSTR && var.bstrVal != nullptr) { - // Check if the device ID contains "IG_". If it does, then itďż˝s an XInput device + // Check if the device ID contains "IG_". If it does, then it's an XInput device // Unfortunately this information can not be found by just using DirectInput if (wcsstr(var.bstrVal, L"IG_")) { @@ -668,7 +667,7 @@ struct DirectInputJoyStick::impl{ // Release any DirectInput objects. DIJT_SAFE_RELEASE(g_pAutoCenterHandle); - DIJT_SAFE_RELEASE(g_pWheelRumbleHandle); + DIJT_SAFE_RELEASE(g_pWheelRumbleHandle); DIJT_SAFE_RELEASE(g_pJoystick); DIJT_SAFE_RELEASE(g_pDI); } diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h index be47f7e004..483b3b0ecf 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoystick.h @@ -43,10 +43,10 @@ class DirectInputJoyStick { ~DirectInputJoyStick(); bool initialize(unsigned int joystick_index); // strength ranges from -1 to 1 - void setAutoCenter(double strength); + void setAutoCenter(double strength); - // strength ranges from 0 to 1 - void setWheelRumble(double strength); + // strength ranges from 0 to 1 + void setWheelRumble(double strength); const JoystickState& getState(bool update_state = true); const Capabilities& getCapabilities(); const JoystickInfo& getJoystickInfo(); diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp index 6156ada4a1..1f402180a5 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp @@ -371,10 +371,10 @@ void SimJoyStick::getJoyStickState(unsigned int index, SimJoyStick::State& state void SimJoyStick::setAutoCenter(unsigned int index, double strength) { - pimpl_->setAutoCenter(index, strength); + pimpl_->setAutoCenter(index, strength); } void SimJoyStick::setWheelRumble(unsigned int index, double strength) { - pimpl_->setWheelRumble(index, strength); + pimpl_->setWheelRumble(index, strength); } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h index 60337dc7f3..a7e6f79c77 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h @@ -45,10 +45,10 @@ class SimJoyStick void getJoyStickState(unsigned int index, State& state); // strength ranges from -1 to 1 - void setAutoCenter(unsigned int index, double strength); + void setAutoCenter(unsigned int index, double strength); - // strength ranges from 0 to 1 - void setWheelRumble(unsigned int index, double strength); + // strength ranges from 0 to 1 + void setWheelRumble(unsigned int index, double strength); SimJoyStick(); ~SimJoyStick(); //required for pimpl From 284aa93270a266fe6e51684ce413047f2087f7c0 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Tue, 17 Apr 2018 12:58:17 +0200 Subject: [PATCH 080/121] encoding/formatiing fix --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 46 +++++++++---------- .../SimJoyStick/DirectInputJoyStick.cpp | 4 +- .../AirSim/Source/SimJoyStick/SimJoyStick.h | 6 +-- 3 files changed, 27 insertions(+), 29 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 5e6031653a..d5836c2895 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -416,18 +416,18 @@ void ACarPawn::updateCarControls() std::string vendorid = joystick_state_.pid_vid.substr(0, joystick_state_.pid_vid.find('&')); - // Thrustmaster devices - if (vendorid == "VID_044F") { - joystick_controls_.steering = joystick_state_.left_x; - joystick_controls_.throttle = (-joystick_state_.right_z + 1) / 2; - joystick_controls_.brake = (joystick_state_.left_y + 1) / 2; - } - // Anything else - else { - joystick_controls_.steering = joystick_state_.left_y * 1.25; - joystick_controls_.throttle = (-joystick_state_.right_x + 1) / 2; - joystick_controls_.brake = -joystick_state_.right_z + 1; - } + // Thrustmaster devices + if (vendorid == "VID_044F") { + joystick_controls_.steering = joystick_state_.left_x; + joystick_controls_.throttle = (-joystick_state_.right_z + 1) / 2; + joystick_controls_.brake = (joystick_state_.left_y + 1) / 2; + } + // Anything else + else { + joystick_controls_.steering = joystick_state_.left_y * 1.25; + joystick_controls_.throttle = (-joystick_state_.right_x + 1) / 2; + joystick_controls_.brake = -joystick_state_.right_z + 1; + } //Two steel levers behind wheel joystick_controls_.handbrake = (joystick_state_.buttons & 32) | (joystick_state_.buttons & 64) ? 1 : 0; @@ -465,21 +465,21 @@ void ACarPawn::updateCarControls() } void ACarPawn::updateForceFeedback() { - if (joystick_state_.is_initialized) { + if (joystick_state_.is_initialized) { - // Update wheel rumble - float rumblestrength = 0.66 + (GetVehicleMovement()->GetEngineRotationSpeed() - / GetVehicleMovement()->GetEngineMaxRotationSpeed()) / 3; + // Update wheel rumble + float rumblestrength = 0.66 + (GetVehicleMovement()->GetEngineRotationSpeed() + / GetVehicleMovement()->GetEngineMaxRotationSpeed()) / 3; - joystick_.setWheelRumble(wrapper_->getRemoteControlID(), rumblestrength); + joystick_.setWheelRumble(wrapper_->getRemoteControlID(), rumblestrength); - // Update autocenter - double speed = GetVehicleMovement()->GetForwardSpeed(); + // Update autocenter + double speed = GetVehicleMovement()->GetForwardSpeed(); - joystick_.setAutoCenter(wrapper_->getRemoteControlID(), - ( 1.0 - 1.0 / ( std::abs(speed / 120) + 1.0)) - * (joystick_state_.left_x / 3)); - } + joystick_.setAutoCenter(wrapper_->getRemoteControlID(), + ( 1.0 - 1.0 / ( std::abs(speed / 120) + 1.0)) + * (joystick_state_.left_x / 3)); + } } diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 5d82536cdc..c826d24477 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -303,7 +303,7 @@ struct DirectInputJoyStick::impl{ //----------------------------------------------------------------------------- // Enum each PNP device using WMI and check each device ID to see if it contains - // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it's an XInput device + // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it’s an XInput device // Unfortunately this information can not be found by just using DirectInput. // Checking against a VID/PID of 0x028E/0x045E won't find 3rd party or future // XInput devices. @@ -378,7 +378,7 @@ struct DirectInputJoyStick::impl{ hr = pDevices[iDevice]->Get(bstrDeviceID, 0L, &var, nullptr, nullptr); if (SUCCEEDED(hr) && var.vt == VT_BSTR && var.bstrVal != nullptr) { - // Check if the device ID contains "IG_". If it does, then it's an XInput device + // Check if the device ID contains "IG_". If it does, then it’s an XInput device // Unfortunately this information can not be found by just using DirectInput if (wcsstr(var.bstrVal, L"IG_")) { diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h index a7e6f79c77..ebcd7755cd 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.h @@ -43,13 +43,12 @@ class SimJoyStick }; void getJoyStickState(unsigned int index, State& state); - // strength ranges from -1 to 1 void setAutoCenter(unsigned int index, double strength); // strength ranges from 0 to 1 void setWheelRumble(unsigned int index, double strength); - + SimJoyStick(); ~SimJoyStick(); //required for pimpl private: @@ -58,5 +57,4 @@ class SimJoyStick struct impl; std::unique_ptr pimpl_; -}; - +}; \ No newline at end of file From 1c4f7e48752283cdbf4a041a2c461b026bfd8896 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Tue, 17 Apr 2018 17:27:23 +0200 Subject: [PATCH 081/121] added RPM to CarState --- AirLib/include/vehicles/car/api/CarApiBase.hpp | 5 ++++- AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp | 8 ++++++-- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp | 2 ++ 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 67b254c5a6..70367b90cb 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -50,13 +50,16 @@ class CarApiBase : public VehicleApiBase { struct CarState { float speed; int gear; + float rpm; + float maxrpm; CollisionInfo collision; Kinematics::State kinematics_true; uint64_t timestamp; CarState(float speed_val, int gear_val, const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, uint64_t timestamp_val) - : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val) + : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val), + rpm(rpm_val), maxrpm(maxrpm_val) { } }; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp index 0f205bf3e6..9a1cf9cfc2 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp @@ -50,11 +50,13 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { struct CarState { float speed; int gear; + float rpm; + float maxrpm; CollisionInfo collision; KinematicsState kinematics_true; //ground truth uint64_t timestamp; - MSGPACK_DEFINE_MAP(speed, gear, collision, kinematics_true, timestamp); + MSGPACK_DEFINE_MAP(speed, gear, rpm, maxrpm, collision, kinematics_true, timestamp); CarState() {} @@ -63,6 +65,8 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { { speed = s.speed; gear = s.gear; + rpm = s.rpm; + maxrpm = s.maxrpm; collision = s.collision; kinematics_true = s.kinematics_true; timestamp = s.timestamp; @@ -70,7 +74,7 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { msr::airlib::CarApiBase::CarState to() const { return msr::airlib::CarApiBase::CarState( - speed, gear, collision.to(), kinematics_true.to(), timestamp); + speed, gear, rpm, maxrpm, collision.to(), kinematics_true.to(), timestamp); } }; }; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index 21f401d618..bc810bbb39 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -104,6 +104,8 @@ CarApiBase::CarState CarPawnApi::getCarState() const CarApiBase::CarState state( movement_->GetForwardSpeed() / 100, //cm/s -> m/s movement_->GetCurrentGear(), + movement_->GetEngineRotationSpeed(), + movement_->GetEngineMaxRotationSpeed(), pawn_->getCollisionInfo(), *pawn_->getTrueKinematics(), msr::airlib::ClockFactory::get()->nowNanos() From d620563a6cd15e90b6323b45a98bd5300f3f2128 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Tue, 17 Apr 2018 17:33:49 +0200 Subject: [PATCH 082/121] CarState constructor fix --- AirLib/include/vehicles/car/api/CarApiBase.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 70367b90cb..832f12d14d 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -56,7 +56,7 @@ class CarApiBase : public VehicleApiBase { Kinematics::State kinematics_true; uint64_t timestamp; - CarState(float speed_val, int gear_val, const CollisionInfo& collision_val, + CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, uint64_t timestamp_val) : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val), rpm(rpm_val), maxrpm(maxrpm_val) From 97459f08d54538f66b725ba0398781d0b70d5926 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 17 Apr 2018 15:23:07 -0700 Subject: [PATCH 083/121] fix orbit.py --- PythonClient/orbit.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index ee5068265c..1acbc82427 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -18,6 +18,8 @@ def __init__(self, args): self.speed = args.speed self.iterations = args.iterations self.snapshots = args.snapshots + self.snapshot_delta = None + self.next_snapshot = None self.z = None self.snapshot_index = 0 @@ -166,7 +168,7 @@ def track_orbits(self, angle): # ignore the click over from 360 back to 0 if self.previous_angle > 350 and angle < 10: - if self.next_snapshot >= 360: + if self.snapshot_delta and self.next_snapshot >= 360: self.next_snapshot -= 360 return False From 4036fa81da4d9cc1d1c247f8f7b1df3ea729748c Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 17 Apr 2018 18:12:09 -0700 Subject: [PATCH 084/121] make orbit.py module easier to reuse from other scripts. --- PythonClient/orbit.py | 58 ++++++++++++++++++++++++------------------- 1 file changed, 33 insertions(+), 25 deletions(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index 1acbc82427..54787bd28f 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -12,16 +12,17 @@ def __init__(self, pos): # Make the drone fly in a circle. class OrbitNavigator: - def __init__(self, args): - self.radius = args.radius - self.altitude = args.altitude - self.speed = args.speed - self.iterations = args.iterations - self.snapshots = args.snapshots + def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterations = 1, center = [1,0], snapshots = None): + self.radius = radius + self.altitude = altitude + self.speed = speed + self.iterations = iterations + self.snapshots = snapshots self.snapshot_delta = None self.next_snapshot = None self.z = None self.snapshot_index = 0 + self.takeoff = takeoff # whether to do the take off and landing. if self.snapshots > 0: self.snapshot_delta = 360 / self.snapshots @@ -29,12 +30,12 @@ def __init__(self, args): if self.iterations <= 0: self.iterations = 1 - p = args.center.split(',') - if len(p) != 2: - raise Exception("Expecting 'x,y' for the center direction vector") + if len(center) != 2: + raise Exception("Expecting '[x,y]' for the center direction vector") - cx = float(p[0]) - cy = float(p[1]) + # center is just a direction vector, so normalize it to compute the actual cx,cy locations. + cx = float(center[0]) + cy = float(center[1]) length = math.sqrt(cx*cx)+(cy*cy) cx /= length cy /= length @@ -75,15 +76,16 @@ def start(self): # AirSim uses NED coordinates so negative axis is up. start = self.getPosition() - z = -self.altitude + self.home.z landed = self.client.getLandedState() - if landed == LandedState.Landed: + if self.takeoff and landed == LandedState.Landed: print("taking off...") self.client.takeoff() + start = self.getPosition() + z = -self.altitude + self.home.z else: - print("already flying so we will orbit at current altitude {}".format(start.z)) + print("already flying so we will orbit at current altitude {}".format(start.z)) z = start.z # use current altitude then - + print("climbing to position: {},{},{}".format(start.x, start.y, z)) self.client.moveToPosition(start.x, start.y, z, self.speed) self.z = z @@ -133,16 +135,22 @@ def start(self): self.camera_heading = camera_heading self.client.moveByVelocityZ(vx, vy, z, 1, DrivetrainType.MaxDegreeOfFreedom, YawMode(False, camera_heading)) - - if z < self.home.z: - print("descending") - self.client.moveToPosition(start.x, start.y, self.home.z - 5, 2) - print("landing...") - self.client.land() - print("disarming.") - self.client.armDisarm(False) + self.client.moveToPosition(start.x, start.y, z, 2) + + if self.takeoff: + # if we did the takeoff then also do the landing. + if z < self.home.z: + print("descending") + self.client.moveToPosition(start.x, start.y, self.home.z - 5, 2) + + print("landing...") + self.client.land() + + print("disarming.") + self.client.armDisarm(False) + def track_orbits(self, angle): # tracking # of completed orbits is surprisingly tricky to get right in order to handle random wobbles @@ -227,6 +235,6 @@ def sign(self, s): arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3) arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0) - args = arg_parser.parse_args(args) - nav = OrbitNavigator(args) + args = arg_parser.parse_args(args) + nav = OrbitNavigator(True, args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots) nav.start() From b06c3ec4758de2b8bd8078f50747f614c29fbebf Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 17 Apr 2018 18:53:23 -0700 Subject: [PATCH 085/121] add commandline --takeoff option to orbit.py. --- PythonClient/orbit.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index 54787bd28f..f9ce9d1836 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -54,7 +54,7 @@ def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterati pos = self.home if abs(pos.z - self.home.z) > 1: count = 0 - home = pos + self.home = pos if time.time() - start > 10: print("Drone position is drifting, we are waiting for it to settle down...") start = time @@ -150,7 +150,7 @@ def start(self): print("disarming.") self.client.armDisarm(False) - + def track_orbits(self, angle): # tracking # of completed orbits is surprisingly tricky to get right in order to handle random wobbles @@ -225,6 +225,9 @@ def sign(self, s): return -1 return 1 +def str2bool(v): + return v.lower() in [ "true", "1", "yes"] + if __name__ == "__main__": args = sys.argv args.pop(0) @@ -234,7 +237,8 @@ def sign(self, s): arg_parser.add_argument("--speed", type=float, help="speed of orbit (in meters/second)", default=3) arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3) - arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0) + arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0) + arg_parser.add_argument("--takeoff", help="Whether to perform takeoff or not", default="True") args = arg_parser.parse_args(args) - nav = OrbitNavigator(True, args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots) + nav = OrbitNavigator(str2bool(args.takeoff), args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots) nav.start() From 22fdd38377ded3d812fcedb2b10e44cabb20b77e Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 17 Apr 2018 20:24:52 -0700 Subject: [PATCH 086/121] implement getLandedState. --- .../SimpleFlightDroneController.hpp | 3 +- .../simple_flight/firmware/OffboardApi.hpp | 40 +++++++++++++++++-- .../firmware/interfaces/IOffboardApi.hpp | 2 + PythonClient/orbit.py | 13 +++--- 4 files changed, 45 insertions(+), 13 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp index 0e7e91ea2c..d9e0a0eb64 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp @@ -143,8 +143,7 @@ class SimpleFlightDroneController : public DroneControllerBase { virtual LandedState getLandedState() const override { - //TODO: implement this - return LandedState::Landed; + return firmware_->offboardApi().getLandedState() ? LandedState::Landed : LandedState::Flying; } virtual int getRemoteControlID() const override diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index 478db49a0d..a5e7435791 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -18,7 +18,7 @@ class OffboardApi : OffboardApi(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs, IStateEstimator* state_estimator, ICommLink* comm_link) : params_(params), rc_(params, clock, board_inputs, &vehicle_state_, state_estimator, comm_link), - state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock) + state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock), landed_(true) { } @@ -28,7 +28,8 @@ class OffboardApi : vehicle_state_.setState(params_->default_vehicle_state, state_estimator_->getGeoPoint()); rc_.reset(); - has_api_control_ = false; + has_api_control_ = false; + landed_ = true; goal_timestamp_ = 0; updateGoalFromRc(); } @@ -46,7 +47,6 @@ class OffboardApi : comm_link_->log("API call timed out, entering hover mode"); goal_mode_ = GoalMode::getPositionMode(); goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true); - is_api_timedout_ = true; } @@ -55,6 +55,8 @@ class OffboardApi : } //else leave the goal set by IOffboardApi API + + detectLanding(); } /**************** IOffboardApi ********************/ @@ -199,6 +201,10 @@ class OffboardApi : return state_estimator_->getGeoPoint(); } + virtual bool getLandedState() const override + { + return landed_; + } private: void updateGoalFromRc() @@ -207,7 +213,34 @@ class OffboardApi : goal_mode_ = rc_.getGoalMode(); } + void detectLanding() { + + // if we are not trying to move by setting motor outputs + if (isAlmostZero(goal_.roll()) && isAlmostZero(goal_.pitch()) && isAlmostZero(goal_.yaw()) && isGreaterThanMinThrottle(goal_.throttle())) + { + // and we are not currently moving (based on current velocities) + auto angular = state_estimator_->getAngulerVelocity(); + auto velocity = state_estimator_->getLinearVelocity(); + if (isAlmostZero(angular.roll()) && isAlmostZero(angular.pitch()) && isAlmostZero(angular.yaw()) && + isAlmostZero(velocity.roll()) && isAlmostZero(velocity.pitch()) && isAlmostZero(velocity.yaw())) { + // then we must be landed... + landed_ = true; + return; + } + } + + landed_ = false; + } + + bool isAlmostZero(float v) { + return std::abs(v) < kMovementTolerance; + } + bool isGreaterThanMinThrottle(float throttle) { + return std::abs(throttle) <= std::abs(params_->rc.min_angling_throttle); + } + private: + const TReal kMovementTolerance = (TReal)0.08; const Params* params_; RemoteControl rc_; IStateEstimator* state_estimator_; @@ -222,6 +255,7 @@ class OffboardApi : bool has_api_control_; bool is_api_timedout_; + bool landed_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp index e69d17d3c3..a8adc2967b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp @@ -18,6 +18,8 @@ class IOffboardApi : public IGoal { virtual bool disarm(std::string& message) = 0; virtual VehicleStateType getVehicleState() const = 0; + virtual bool getLandedState() const = 0; + virtual const IStateEstimator& getStateEstimator() = 0; virtual GeoPoint getHomeGeoPoint() const = 0; virtual GeoPoint getGeoPoint() const = 0; diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index f9ce9d1836..9d97a3b822 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -12,7 +12,7 @@ def __init__(self, pos): # Make the drone fly in a circle. class OrbitNavigator: - def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterations = 1, center = [1,0], snapshots = None): + def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center = [1,0], snapshots = None): self.radius = radius self.altitude = altitude self.speed = speed @@ -22,7 +22,7 @@ def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterati self.next_snapshot = None self.z = None self.snapshot_index = 0 - self.takeoff = takeoff # whether to do the take off and landing. + self.takeoff = False # whether we did a take off if self.snapshots > 0: self.snapshot_delta = 360 / self.snapshots @@ -77,7 +77,8 @@ def start(self): # AirSim uses NED coordinates so negative axis is up. start = self.getPosition() landed = self.client.getLandedState() - if self.takeoff and landed == LandedState.Landed: + if landed == LandedState.Landed: + self.takeoff = True print("taking off...") self.client.takeoff() start = self.getPosition() @@ -225,9 +226,6 @@ def sign(self, s): return -1 return 1 -def str2bool(v): - return v.lower() in [ "true", "1", "yes"] - if __name__ == "__main__": args = sys.argv args.pop(0) @@ -238,7 +236,6 @@ def str2bool(v): arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0") arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3) arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0) - arg_parser.add_argument("--takeoff", help="Whether to perform takeoff or not", default="True") args = arg_parser.parse_args(args) - nav = OrbitNavigator(str2bool(args.takeoff), args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots) + nav = OrbitNavigator(args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots) nav.start() From f303e613556ba470d257bef1fea05ea6da654535 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 18 Apr 2018 12:02:39 -0700 Subject: [PATCH 087/121] update docs to say we can use latest PX4 firmware. --- docs/px4_setup.md | 6 +++--- docs/px4_sitl.md | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/docs/px4_setup.md b/docs/px4_setup.md index 4c21c81c74..2303729800 100644 --- a/docs/px4_setup.md +++ b/docs/px4_setup.md @@ -2,7 +2,7 @@ The [PX4 software stack](http://github.com/px4/firmware) is an open source very popular flight controller with support for wide variety of boards and sensors as well as built-in capability for higher level tasks such as mission planning. Please visit [px4.io](http://px4.io) for more information. -**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim. Due to bugs [such as this](https://github.com/PX4/Firmware/issues/7516) things haven't been working after PX4 release 1.5.5. With that release, you may still see wobbly drone sometimes. For issues related to PX4 setup please visit [PX4 forums](http://discuss.px4.io/). +**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim. ## Supported Hardware @@ -15,7 +15,7 @@ The following pixhawk hardware has been tested with AirSim: 4. [PixRacer](https://www.banggood.com/Pixracer-Autopilot-Xracer-V1_0-Flight-Controller-Mini-PX4-Built-in-Wifi-For-FPV-Racing-RC-Multirotor-p-1056428.html?utm_source=google&utm_medium=cpc_ods&utm_content=starr&utm_campaign=Smlrfpv-ds-FPVracer&gclid=CjwKEAjw9MrIBRCr2LPek5-h8U0SJAD3jfhtbEfqhX4Lu94kPe88Zrr62A5qVgx-wRDBuUulGzHELRoCRVTw_wcB) 5. [Pixhawk 2.1](http://www.proficnc.com/) (using PX4 Flight Stack) -The 3DR Pixhawk Mini works out of the box, the others you may need to re-flash with the firmware v1.5.5. +The 3DR Pixhawk Mini works out of the box, the others you may need to re-flash with the latest firmware. ## Setting up PX4 Hardware-in-Loop @@ -53,7 +53,7 @@ The PX4 SITL mode doesn't require you to have separate device such as Pixhawk or There are few reasons that can cause this. First make sure your drone doesn't fall down large distance when starting simulator. This might happen if you have created custom Unreal environment and Player Start is placed too high above ground. It seems that when this happens internal calibration in PX4 gets confused. -Another thing to verify is to make sure you are using PX4 release 1.5.5 as later releases seems to have few issues. You should also use QGroundControl and make sure you can arm and takeoff in QGroundControl properly. +You should also use QGroundControl and make sure you can arm and takeoff in QGroundControl properly. Finally, this also can be a machine performance issue in some rare cases, check your [hard drive performance](hard_drive.md). diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index ec82f1bca1..c743cd6901 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -11,7 +11,6 @@ you can build and run it there. cd PX4 git clone https://github.com/PX4/Firmware.git cd Firmware - git checkout tags/v1.5.5 -b v1.5.5 make posix_sitl_default ``` 3. Use following command to start PX4 firmware in SITL mode: From e91e0f131d17d6e4ce03c0b1ac0040941efd9185 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Wed, 18 Apr 2018 12:30:07 -0700 Subject: [PATCH 088/121] Fix typo in name of method getAngulerVelocity. --- .../firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp | 4 ++-- .../firmwares/simple_flight/firmware/AdaptiveController.hpp | 2 +- .../firmwares/simple_flight/firmware/AngleRateController.hpp | 2 +- .../firmwares/simple_flight/firmware/OffboardApi.hpp | 2 +- .../simple_flight/firmware/interfaces/IStateEstimator.hpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index ed77c6bb96..7cbe25e63a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -33,7 +33,7 @@ class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator { return angles; } - virtual simple_flight::Axis3r getAngulerVelocity() const override + virtual simple_flight::Axis3r getAngularVelocity() const override { const auto& anguler = kinematics_->twist.angular; @@ -86,7 +86,7 @@ class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator { state.position = getPosition(); state.orientation = getOrientation(); state.linear_velocity = getLinearVelocity(); - state.angular_velocity = getAngulerVelocity(); + state.angular_velocity = getAngularVelocity(); state.linear_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.linear); state.angular_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.angular); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp index f602549130..4da9f86787 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AdaptiveController.hpp @@ -37,7 +37,7 @@ class AdaptiveController : public IController { // Assign state variables to placeholder variables angle = state_estimator_->getAngles(); - angle_rate = state_estimator_->getAngulerVelocity(); + angle_rate = state_estimator_->getAngularVelocity(); position = state_estimator_->getPosition(); velocity = state_estimator_->getLinearVelocity(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp index 4e12c7dd0a..e961c963f6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp @@ -47,7 +47,7 @@ class AngleRateController : public IAxisController { IAxisController::update(); pid_->setGoal(goal_->getGoalValue()[axis_]); - pid_->setMeasured(state_estimator_->getAngulerVelocity()[axis_]); + pid_->setMeasured(state_estimator_->getAngularVelocity()[axis_]); pid_->update(); output_ = pid_->getOutput(); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index a5e7435791..5018983ab9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -219,7 +219,7 @@ class OffboardApi : if (isAlmostZero(goal_.roll()) && isAlmostZero(goal_.pitch()) && isAlmostZero(goal_.yaw()) && isGreaterThanMinThrottle(goal_.throttle())) { // and we are not currently moving (based on current velocities) - auto angular = state_estimator_->getAngulerVelocity(); + auto angular = state_estimator_->getAngularVelocity(); auto velocity = state_estimator_->getLinearVelocity(); if (isAlmostZero(angular.roll()) && isAlmostZero(angular.pitch()) && isAlmostZero(angular.yaw()) && isAlmostZero(velocity.roll()) && isAlmostZero(velocity.pitch()) && isAlmostZero(velocity.yaw())) { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp index d2a393d3b3..d3cd866ee4 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp @@ -7,7 +7,7 @@ namespace simple_flight { class IStateEstimator { public: virtual Axis3r getAngles() const = 0; - virtual Axis3r getAngulerVelocity() const = 0; + virtual Axis3r getAngularVelocity() const = 0; virtual Axis3r getPosition() const = 0; virtual Axis3r getLinearVelocity() const = 0; virtual Axis4r getOrientation() const = 0; From 04691edf2afd10bd8f1d2cd8e639b66723b1d327 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Fri, 20 Apr 2018 18:54:06 +0200 Subject: [PATCH 089/121] Added handbrake to CarState --- AirLib/include/vehicles/car/api/CarApiBase.hpp | 5 +++-- .../vehicles/car/api/CarRpcLibAdapators.hpp | 6 ++++-- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp | 16 +++++++++------- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 832f12d14d..6a26135baf 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -52,14 +52,15 @@ class CarApiBase : public VehicleApiBase { int gear; float rpm; float maxrpm; + bool handbrake; CollisionInfo collision; Kinematics::State kinematics_true; uint64_t timestamp; - CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, const CollisionInfo& collision_val, + CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, bool handbrake_val, const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, uint64_t timestamp_val) : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val), - rpm(rpm_val), maxrpm(maxrpm_val) + rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val) { } }; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp index 9a1cf9cfc2..b84923d19c 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp @@ -52,11 +52,12 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { int gear; float rpm; float maxrpm; + bool handbrake; CollisionInfo collision; KinematicsState kinematics_true; //ground truth uint64_t timestamp; - MSGPACK_DEFINE_MAP(speed, gear, rpm, maxrpm, collision, kinematics_true, timestamp); + MSGPACK_DEFINE_MAP(speed, gear, rpm, maxrpm, handbrake, collision, kinematics_true, timestamp); CarState() {} @@ -67,6 +68,7 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { gear = s.gear; rpm = s.rpm; maxrpm = s.maxrpm; + handbrake = s.handbrake; collision = s.collision; kinematics_true = s.kinematics_true; timestamp = s.timestamp; @@ -74,7 +76,7 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { msr::airlib::CarApiBase::CarState to() const { return msr::airlib::CarApiBase::CarState( - speed, gear, rpm, maxrpm, collision.to(), kinematics_true.to(), timestamp); + speed, gear, rpm, maxrpm, handbrake, collision.to(), kinematics_true.to(), timestamp); } }; }; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index bc810bbb39..3646c0d8fb 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -57,7 +57,8 @@ void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls) { if (api_control_enabled_) last_controls_ = controls; - //else don't save + //else only save handbrake state + else last_controls_.handbrake = controls.handbrake; if (!controls.is_manual_gear && movement_->GetTargetGear() < 0) movement_->SetTargetGear(0, true); //in auto gear we must have gear >= 0 @@ -101,12 +102,13 @@ void CarPawnApi::setCameraOrientation(int camera_id, const msr::airlib::Quaterni CarApiBase::CarState CarPawnApi::getCarState() const { - CarApiBase::CarState state( - movement_->GetForwardSpeed() / 100, //cm/s -> m/s - movement_->GetCurrentGear(), - movement_->GetEngineRotationSpeed(), - movement_->GetEngineMaxRotationSpeed(), - pawn_->getCollisionInfo(), + CarApiBase::CarState state( + movement_->GetForwardSpeed() / 100, //cm/s -> m/s + movement_->GetCurrentGear(), + movement_->GetEngineRotationSpeed(), + movement_->GetEngineMaxRotationSpeed(), + last_controls_.handbrake, + pawn_->getCollisionInfo(), *pawn_->getTrueKinematics(), msr::airlib::ClockFactory::get()->nowNanos() ); From 1572f628529724926251b8fdd3687c9f4b559d00 Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Sat, 21 Apr 2018 14:55:48 +0200 Subject: [PATCH 090/121] added setAutoCenter implementation --- Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp index 1f402180a5..20b637b0cd 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp @@ -51,6 +51,14 @@ struct SimJoyStick::impl { } } + void setAutoCenter(unsigned int index, double strength) { + controllers_[index]->setAutoCenter(strength); + } + + void setWheelRumble(unsigned int index, double strength) { + controllers_[index]->setWheelRumble(strength); + } + private: float getMappedValue(AxisMap::AxisType axis_type, const AxisMap& map, const DirectInputJoyStick::JoystickState& di_state, const std::string& device_pid_vid) From 2df8c09e9f1c5cd1e1a4b70b00cc00e78516a4df Mon Sep 17 00:00:00 2001 From: "DOLICHUS\\kisss" Date: Mon, 23 Apr 2018 13:16:11 +0200 Subject: [PATCH 091/121] added FFE strength validation --- .../AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index c826d24477..7f16bfda5f 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -14,6 +14,7 @@ #pragma warning(pop) #include +#include // Stuff to filter out XInput devices #ifndef FALSE @@ -689,6 +690,14 @@ bool DirectInputJoyStick::initialize(unsigned int joystick_index) { return pimpl_->initialize(joystick_index); } +void DirectInputJoyStick::setAutoCenter(double strength) +{ + pimpl_->setAutoCenterStrength(FMath::Clamp(strength, -1.0, 1.0)); +} +void DirectInputJoyStick::setWheelRumble(double strength) +{ + pimpl_->setWheelRumbleStrength(FMath::Clamp(strength, 0.0, 1.0)); +} const DirectInputJoyStick::JoystickState& DirectInputJoyStick::getState(bool update_state) { return pimpl_->getState(update_state); From 2ac10fb9bfa2450f09b7e00d736e20da2964ebc4 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 23 Apr 2018 11:30:11 -0700 Subject: [PATCH 092/121] Minor changes to PR https://github.com/Microsoft/AirSim/pull/1000, PythonClient update --- .../include/vehicles/car/api/CarApiBase.hpp | 2 +- .../vehicles/car/api/CarRpcLibAdapators.hpp | 4 ++-- PythonClient/AirSimClient.py | 3 +++ Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 2 ++ .../Plugins/AirSim/Source/Car/CarPawnApi.cpp | 19 ++++++++----------- 5 files changed, 16 insertions(+), 14 deletions(-) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 6a26135baf..a80d9670cb 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -52,7 +52,7 @@ class CarApiBase : public VehicleApiBase { int gear; float rpm; float maxrpm; - bool handbrake; + bool handbrake; CollisionInfo collision; Kinematics::State kinematics_true; uint64_t timestamp; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp index b84923d19c..b527ddf489 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp @@ -52,7 +52,7 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { int gear; float rpm; float maxrpm; - bool handbrake; + bool handbrake; CollisionInfo collision; KinematicsState kinematics_true; //ground truth uint64_t timestamp; @@ -68,7 +68,7 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase { gear = s.gear; rpm = s.rpm; maxrpm = s.maxrpm; - handbrake = s.handbrake; + handbrake = s.handbrake; collision = s.collision; kinematics_true = s.kinematics_true; timestamp = s.timestamp; diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 78652e2fed..fbdcbc00c7 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -191,6 +191,9 @@ class KinematicsState(MsgpackMixin): class CarState(MsgpackMixin): speed = np.float32(0) gear = 0 + rpm = np.float32(0) + maxrpm = np.float32(0) + handbrake = False collision = CollisionInfo(); kinematics_true = KinematicsState() timestamp = np.uint64(0) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 53207d318b..ad3a4f217b 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -438,7 +438,9 @@ void ACarPawn::updateCarControls() current_controls_ = keyboard_controls_; } + //if API-client control is not active then we route keyboard/jostick control to car if (!api_->isApiControlEnabled()) { + //all car controls from anywhere must be routed through API component api_->setCarControls(current_controls_); } else { diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index 3646c0d8fb..9c826621c7 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -55,10 +55,7 @@ std::vector CarPawnApi::simGetImage(uint8_t camera_id, ImageCaptureBase void CarPawnApi::setCarControls(const CarApiBase::CarControls& controls) { - if (api_control_enabled_) - last_controls_ = controls; - //else only save handbrake state - else last_controls_.handbrake = controls.handbrake; + last_controls_ = controls; if (!controls.is_manual_gear && movement_->GetTargetGear() < 0) movement_->SetTargetGear(0, true); //in auto gear we must have gear >= 0 @@ -102,13 +99,13 @@ void CarPawnApi::setCameraOrientation(int camera_id, const msr::airlib::Quaterni CarApiBase::CarState CarPawnApi::getCarState() const { - CarApiBase::CarState state( - movement_->GetForwardSpeed() / 100, //cm/s -> m/s - movement_->GetCurrentGear(), - movement_->GetEngineRotationSpeed(), - movement_->GetEngineMaxRotationSpeed(), - last_controls_.handbrake, - pawn_->getCollisionInfo(), + CarApiBase::CarState state( + movement_->GetForwardSpeed() / 100, //cm/s -> m/s + movement_->GetCurrentGear(), + movement_->GetEngineRotationSpeed(), + movement_->GetEngineMaxRotationSpeed(), + last_controls_.handbrake, + pawn_->getCollisionInfo(), *pawn_->getTrueKinematics(), msr::airlib::ClockFactory::get()->nowNanos() ); From 57de2f28dea7e15f0374ca368bee5537ed701519 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 23 Apr 2018 13:15:10 -0700 Subject: [PATCH 093/121] minor changes to Thrustmaster support PR https://github.com/Microsoft/AirSim/pull/979 --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 2 +- .../SimJoyStick/DirectInputJoyStick.cpp | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index c981ba658a..ec4f8b3853 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -422,7 +422,7 @@ void ACarPawn::updateCarControls() joystick_controls_.throttle = (-joystick_state_.right_z + 1) / 2; joystick_controls_.brake = (joystick_state_.left_y + 1) / 2; } - // Anything else + // Anything else, typically Logitech G920 wheel else { joystick_controls_.steering = joystick_state_.left_y * 1.25; joystick_controls_.throttle = (-joystick_state_.right_x + 1) / 2; diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 7f16bfda5f..e3f9d83fc5 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -14,7 +14,7 @@ #pragma warning(pop) #include -#include +#include "UnrealMathUtility.h // Stuff to filter out XInput devices #ifndef FALSE @@ -73,7 +73,8 @@ struct DirectInputJoyStick::impl{ } // Magnitude ranges from -1 to 1 - void setAutoCenterStrength(double magnitude) { + void setAutoCenterStrength(double magnitude) + { DICONSTANTFORCE cf = { magnitude * 10000 }; g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); @@ -86,7 +87,8 @@ struct DirectInputJoyStick::impl{ #define FFWRMAX 0.08 // Strength ranges from 0 to 1 - void setWheelRumbleStrength(double strength) { + void setWheelRumbleStrength(double strength) + { DIPERIODIC pf = { FFWRMAX * strength * 10000,0,0,0.06 * 1000000 }; g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); @@ -136,7 +138,8 @@ struct DirectInputJoyStick::impl{ return g; } - HRESULT InitForceFeedback() { + HRESULT InitForceFeedback() + { HRESULT hr; DWORD rgdwAxes[2] = { DIJOFS_X, DIJOFS_Y }; @@ -304,7 +307,7 @@ struct DirectInputJoyStick::impl{ //----------------------------------------------------------------------------- // Enum each PNP device using WMI and check each device ID to see if it contains - // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it’s an XInput device + // "IG_" (ex. "VID_045E&PID_028E&IG_00"). If it does, then it's an XInput device // Unfortunately this information can not be found by just using DirectInput. // Checking against a VID/PID of 0x028E/0x045E won't find 3rd party or future // XInput devices. @@ -379,7 +382,7 @@ struct DirectInputJoyStick::impl{ hr = pDevices[iDevice]->Get(bstrDeviceID, 0L, &var, nullptr, nullptr); if (SUCCEEDED(hr) && var.vt == VT_BSTR && var.bstrVal != nullptr) { - // Check if the device ID contains "IG_". If it does, then it’s an XInput device + // Check if the device ID contains "IG_". If it does, then it's an XInput device // Unfortunately this information can not be found by just using DirectInput if (wcsstr(var.bstrVal, L"IG_")) { @@ -692,11 +695,11 @@ bool DirectInputJoyStick::initialize(unsigned int joystick_index) } void DirectInputJoyStick::setAutoCenter(double strength) { - pimpl_->setAutoCenterStrength(FMath::Clamp(strength, -1.0, 1.0)); + pimpl_->setAutoCenterStrength(FMath::Clamp(strength, -1.0, 1.0)); } void DirectInputJoyStick::setWheelRumble(double strength) { - pimpl_->setWheelRumbleStrength(FMath::Clamp(strength, 0.0, 1.0)); + pimpl_->setWheelRumbleStrength(FMath::Clamp(strength, 0.0, 1.0)); } const DirectInputJoyStick::JoystickState& DirectInputJoyStick::getState(bool update_state) { From 9850a08b877f22a61e4a5d72ebcdcc945d45f3e4 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 23 Apr 2018 17:45:49 -0700 Subject: [PATCH 094/121] API framework refactoring, Part 1: pawn wrapper provides API --- AirLib/AirLib.vcxproj | 1 + AirLib/AirLib.vcxproj.filters | 3 +++ AirLib/include/api/SimModeApiBase.hpp | 21 +++++++++++++++++ PythonClient/PythonClient.pyproj | 2 +- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 23 +++++++++++-------- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 2 +- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 1 + .../Source/Multirotor/MultiRotorConnector.cpp | 14 +++++++---- .../Source/Multirotor/MultiRotorConnector.h | 2 +- .../Multirotor/SimModeWorldMultiRotor.cpp | 2 ++ .../Multirotor/SimModeWorldMultiRotor.h | 1 + .../SimJoyStick/DirectInputJoyStick.cpp | 2 +- .../AirSim/Source/SimMode/SimModeApi.cpp | 11 +++++++++ .../AirSim/Source/SimMode/SimModeApi.h | 13 +++++++++++ .../AirSim/Source/SimMode/SimModeBase.cpp | 9 ++++++++ .../AirSim/Source/SimMode/SimModeBase.h | 1 + .../AirSim/Source/VehiclePawnWrapper.cpp | 10 ++++++++ .../AirSim/Source/VehiclePawnWrapper.h | 5 ++++ 18 files changed, 106 insertions(+), 17 deletions(-) create mode 100644 AirLib/include/api/SimModeApiBase.hpp create mode 100644 Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp create mode 100644 Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 786bd44290..43ab0d3acf 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -27,6 +27,7 @@ + diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index e5b6be1add..1969184c01 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -489,6 +489,9 @@ Header Files + + Header Files + diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp new file mode 100644 index 0000000000..35e8dfd91e --- /dev/null +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -0,0 +1,21 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef air_SimModeApiBase_hpp +#define air_SimModeApiBase_hpp + +#include "common/CommonStructs.hpp" +#include "VehicleApiBase.hpp" + +namespace msr { namespace airlib { + + +class SimModeApiBase { +public: + virtual VehicleApiBase* getVehicleApi() = 0; + virtual ~SimModeApiBase() = default; +}; + + +}} //namespace +#endif diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index d48133f253..568a9ebb03 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - hello_car.py + hello_drone.py . diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index ec4f8b3853..8d86c20cd3 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -217,22 +217,27 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se void ACarPawn::reset(bool disable_api_control) { keyboard_controls_ = joystick_controls_ = CarPawnApi::CarControls(); - api_->reset(); + wrapper_->setApi(std::unique_ptr()); if (disable_api_control) - api_->enableApiControl(false); + getApi()->enableApiControl(false); +} + +msr::airlib::CarApiBase* ACarPawn::getApi() const +{ + return static_cast(wrapper_->getApi()); } void ACarPawn::startApiServer(bool enable_rpc, const std::string& api_server_address) { if (enable_rpc) { - api_.reset(new CarPawnApi(getVehiclePawnWrapper(), this->GetVehicleMovement())); - + wrapper_->setApi(std::unique_ptr( + new CarPawnApi(getVehiclePawnWrapper(), this->GetVehicleMovement()))); #ifdef AIRLIB_NO_RPC rpclib_server_.reset(new msr::airlib::DebugApiServer()); #else - rpclib_server_.reset(new msr::airlib::CarRpcLibServer(api_.get(), api_server_address)); + rpclib_server_.reset(new msr::airlib::CarRpcLibServer(getApi(), api_server_address)); #endif rpclib_server_->start(); @@ -248,7 +253,7 @@ void ACarPawn::stopApiServer() if (rpclib_server_ != nullptr) { rpclib_server_->stop(); rpclib_server_.reset(nullptr); - api_.reset(nullptr); + wrapper_->setApi(std::unique_ptr()); } } @@ -451,13 +456,13 @@ void ACarPawn::updateCarControls() } //if API-client control is not active then we route keyboard/jostick control to car - if (!api_->isApiControlEnabled()) { + if (! getApi()->isApiControlEnabled()) { //all car controls from anywhere must be routed through API component - api_->setCarControls(current_controls_); + getApi()->setCarControls(current_controls_); } else { UAirBlueprintLib::LogMessageString("Control Mode: ", "API", LogDebugLevel::Informational); - current_controls_ = api_->getCarControls(); + current_controls_ = getApi()->getCarControls(); } UAirBlueprintLib::LogMessageString("Accel: ", std::to_string(current_controls_.throttle), LogDebugLevel::Informational); UAirBlueprintLib::LogMessageString("Break: ", std::to_string(current_controls_.brake), LogDebugLevel::Informational); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index c4e096dd90..2e714b6b14 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -133,6 +133,7 @@ class ACarPawn : public AWheeledVehicle std::string getLogString(); void setupVehicleMovementComponent(); + msr::airlib::CarApiBase* getApi() const; private: @@ -141,7 +142,6 @@ class ACarPawn : public AWheeledVehicle UClass* pip_camera_class_; std::unique_ptr rpclib_server_; - std::unique_ptr api_; std::unique_ptr wrapper_; msr::airlib::Kinematics::State kinematics_; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 62c73591ca..6baf3720d1 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -28,6 +28,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual void reset() override; virtual std::string getReport() override; + private: void setupVehiclesAndCamera(std::vector& vehicles); void updateReport(); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index a094fecee4..9545d28238 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -327,16 +327,22 @@ void MultiRotorConnector::setCameraOrientation(int camera_id, const Quaternionr& }, true); } +msr::airlib::MultirotorApi* MultiRotorConnector::getApi() const +{ + return static_cast(vehicle_pawn_wrapper_->getApi()); +} + void MultiRotorConnector::startApiServer() { if (enable_rpc_) { - controller_cancelable_.reset(new msr::airlib::MultirotorApi(this)); + vehicle_pawn_wrapper_->setApi( + std::unique_ptr(new msr::airlib::MultirotorApi(this))); #ifdef AIRLIB_NO_RPC rpclib_server_.reset(new msr::airlib::DebugApiServer()); #else rpclib_server_.reset(new msr::airlib::MultirotorRpcLibServer( - controller_cancelable_.get(), api_server_address_, api_server_port_)); + getApi(), api_server_address_, api_server_port_)); #endif rpclib_server_->start(); @@ -350,10 +356,10 @@ void MultiRotorConnector::startApiServer() void MultiRotorConnector::stopApiServer() { if (rpclib_server_ != nullptr) { - controller_cancelable_->cancelAllTasks(); + getApi()->cancelAllTasks(); rpclib_server_->stop(); rpclib_server_.reset(nullptr); - controller_cancelable_.reset(nullptr); + vehicle_pawn_wrapper_->setApi(std::unique_ptr()); } } diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h index 7ccd1bb8c7..1efadfcfa9 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h @@ -73,6 +73,7 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase void detectUsbRc(); const msr::airlib::RCData& getRCData(); void resetPrivate(); + msr::airlib::MultirotorApi* getApi() const; private: MultiRotor vehicle_; @@ -81,7 +82,6 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase VehiclePawnWrapper* vehicle_pawn_wrapper_; msr::airlib::MultiRotorParams* vehicle_params_; - std::unique_ptr controller_cancelable_; std::unique_ptr rpclib_server_; struct RotorInfo { diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index b115cd3c5e..30029c972b 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -93,6 +93,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve } } + //find all vehicle pawns { TArray pawns; @@ -143,6 +144,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve CameraDirector->initializeForBeginPlay(getInitialViewMode(), fpv_vehicle_pawn_wrapper_, external_camera); } + void ASimModeWorldMultiRotor::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index f304e1cb80..b5fd2266f7 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -23,6 +23,7 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase VehiclePawnWrapper* getFpvVehiclePawnWrapper() const override; std::string getLogString() const; + protected: typedef AFlyingPawn TMultiRotorPawn; diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index e3f9d83fc5..28d939593f 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -14,7 +14,7 @@ #pragma warning(pop) #include -#include "UnrealMathUtility.h +#include "UnrealMathUtility.h" // Stuff to filter out XInput devices #ifndef FALSE diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp new file mode 100644 index 0000000000..48332f770b --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp @@ -0,0 +1,11 @@ +#include "SimModeApi.h" + +SimModeApi::SimModeApi(ASimModeBase* simmode) + : simmode_(simmode) +{ +} + +msr::airlib::VehicleApiBase* SimModeApi::getVehicleApi() +{ + return simmode_->getVehicleApi(); +} diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h new file mode 100644 index 0000000000..541bcc279e --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h @@ -0,0 +1,13 @@ +#pragma once + +#include "api/SimModeApiBase.hpp" +#include "SimMode/SimModeBase.h" + +class SimModeApi : msr::airlib::SimModeApiBase { +public: + SimModeApi(ASimModeBase* simmode); + virtual msr::airlib::VehicleApiBase* getVehicleApi() override; + +private: + ASimModeBase* simmode_; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index e73e773d72..f604e8b933 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -63,6 +63,7 @@ void ASimModeBase::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } + void ASimModeBase::setupTimeOfDay() { sky_sphere_ = nullptr; @@ -101,6 +102,14 @@ void ASimModeBase::setupTimeOfDay() //else ignore } +msr::airlib::VehicleApiBase* ASimModeBase::getVehicleApi() const +{ + auto fpv_vehicle = getFpvVehiclePawnWrapper(); + if (fpv_vehicle) + return fpv_vehicle->getApi(); + else + return nullptr; +} void ASimModeBase::setupClockSpeed() { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index e8dfb5d0f6..f76a09b793 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -47,6 +47,7 @@ class AIRSIM_API ASimModeBase : public AActor //must be implemented by derived class //can't use pure virtual because of restriction with Unreal virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper() const; + virtual msr::airlib::VehicleApiBase* getVehicleApi() const; protected: diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index 52ad1697a0..51dc873a5b 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -97,6 +97,16 @@ void VehiclePawnWrapper::setKinematics(const msr::airlib::Kinematics::State* kin kinematics_ = kinematics; } +msr::airlib::VehicleApiBase* VehiclePawnWrapper::getApi() const +{ + return api_.get(); +} + +void VehiclePawnWrapper::setApi(std::unique_ptr api) +{ + api_ = std::move(api); +} + void VehiclePawnWrapper::getRawVehicleSettings(msr::airlib::Settings& settings) const { typedef msr::airlib::AirSimSettings AirSimSettings; diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index d8449549ec..554ac3796c 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h @@ -9,6 +9,7 @@ #include "PIPCamera.h" #include "physics/Kinematics.hpp" #include "NedTransform.h" +#include "api/VehicleApiBase.hpp" #include "GameFramework/Pawn.h" class VehiclePawnWrapper @@ -94,6 +95,9 @@ class VehiclePawnWrapper void possess(); + msr::airlib::VehicleApiBase* getApi() const; + void setApi(std::unique_ptr api); + protected: UPROPERTY(VisibleAnywhere) UParticleSystem* collision_display_template; @@ -120,6 +124,7 @@ class VehiclePawnWrapper std::string log_line_; WrapperConfig config_; NedTransform ned_transform_; + std::unique_ptr api_; struct State { FVector start_location; From 62f2a8200bb33b87b814de9697dd3021088a7351 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 24 Apr 2018 00:07:12 -0700 Subject: [PATCH 095/121] API framework refactoring, Part 2: API wiring from SimMode functional --- AirLib/AirLib.vcxproj | 3 +- AirLib/AirLib.vcxproj.filters | 9 +- ...ontrolServerBase.hpp => ApiServerBase.hpp} | 8 +- AirLib/include/api/DebugApiServer.hpp | 4 +- AirLib/include/api/RpcLibServerBase.hpp | 17 +-- .../controllers/VehicleConnectorBase.hpp | 4 - .../vehicles/car/api/CarRpcLibServer.hpp | 4 +- .../multirotor/api/MultirotorRpcLibServer.hpp | 4 +- .../controllers/RealMultirotorConnector.hpp | 13 -- AirLib/src/api/RpcLibServerBase.cpp | 46 +++---- .../src/vehicles/car/api/CarRpcLibServer.cpp | 8 +- .../multirotor/api/MultirotorRpcLibServer.cpp | 8 +- DroneServer/main.cpp | 23 +++- PythonClient/PythonClient.pyproj | 5 +- PythonClient/clock_speed.py | 21 ++++ Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 42 +------ Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 8 +- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 24 +++- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 1 + .../Source/Multirotor/MultiRotorConnector.cpp | 112 +++++------------- .../Source/Multirotor/MultiRotorConnector.h | 14 +-- .../Multirotor/SimModeWorldMultiRotor.cpp | 37 +++--- .../Multirotor/SimModeWorldMultiRotor.h | 1 + .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 37 ++++++ Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h | 7 ++ .../AirSim/Source/SimMode/SimModeApi.cpp | 11 -- .../AirSim/Source/SimMode/SimModeApi.h | 13 -- .../AirSim/Source/SimMode/SimModeBase.cpp | 25 ++++ .../AirSim/Source/SimMode/SimModeBase.h | 15 ++- .../AirSim/Source/SimMode/SimModeWorldBase.h | 2 +- 30 files changed, 273 insertions(+), 253 deletions(-) rename AirLib/include/api/{ControlServerBase.hpp => ApiServerBase.hpp} (68%) create mode 100644 PythonClient/clock_speed.py delete mode 100644 Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp delete mode 100644 Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 43ab0d3acf..8e9b6d42e2 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -22,10 +22,12 @@ + + @@ -132,7 +134,6 @@ - diff --git a/AirLib/AirLib.vcxproj.filters b/AirLib/AirLib.vcxproj.filters index 1969184c01..c4231bfeaa 100644 --- a/AirLib/AirLib.vcxproj.filters +++ b/AirLib/AirLib.vcxproj.filters @@ -210,9 +210,6 @@ Header Files - - Header Files - Header Files @@ -492,6 +489,12 @@ Header Files + + Header Files + + + Header Files + diff --git a/AirLib/include/api/ControlServerBase.hpp b/AirLib/include/api/ApiServerBase.hpp similarity index 68% rename from AirLib/include/api/ControlServerBase.hpp rename to AirLib/include/api/ApiServerBase.hpp index 240c59b38c..5ddeb9cae1 100644 --- a/AirLib/include/api/ControlServerBase.hpp +++ b/AirLib/include/api/ApiServerBase.hpp @@ -1,8 +1,8 @@ // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. -#ifndef air_ControlServerBase_hpp -#define air_ControlServerBase_hpp +#ifndef air_ApiServerBase_hpp +#define air_ApiServerBase_hpp #include "common/Common.hpp" #include @@ -10,11 +10,11 @@ namespace msr { namespace airlib { -class ControlServerBase { +class ApiServerBase { public: virtual void start(bool block = false) = 0; virtual void stop() = 0; - virtual ~ControlServerBase() = default; + virtual ~ApiServerBase() = default; }; }} //namespace diff --git a/AirLib/include/api/DebugApiServer.hpp b/AirLib/include/api/DebugApiServer.hpp index 4212b6ff59..5c826383f0 100644 --- a/AirLib/include/api/DebugApiServer.hpp +++ b/AirLib/include/api/DebugApiServer.hpp @@ -4,12 +4,12 @@ #ifndef air_DebugApiServer_hpp #define air_DebugApiServer_hpp -#include "ControlServerBase.hpp" +#include "ApiServerBase.hpp" #include "common/common_utils/Utils.hpp" namespace msr { namespace airlib { - class DebugApiServer : public ControlServerBase { + class DebugApiServer : public ApiServerBase { public: virtual void start(bool block = false) override { diff --git a/AirLib/include/api/RpcLibServerBase.hpp b/AirLib/include/api/RpcLibServerBase.hpp index b29923fd7e..4dd279e72c 100644 --- a/AirLib/include/api/RpcLibServerBase.hpp +++ b/AirLib/include/api/RpcLibServerBase.hpp @@ -5,26 +5,29 @@ #define air_RpcLibServerBase_hpp #include "common/Common.hpp" -#include "api/ControlServerBase.hpp" -#include "api/VehicleApiBase.hpp" +#include "api/ApiServerBase.hpp" +#include "api/SimModeAPiBase.hpp" namespace msr { namespace airlib { -class RpcLibServerBase : public ControlServerBase { +class RpcLibServerBase : public ApiServerBase { public: - RpcLibServerBase(VehicleApiBase* vehicle, string server_address, uint16_t port); + RpcLibServerBase(SimModeApiBase* simmode_api, string server_address, uint16_t port); virtual void start(bool block = false) override; virtual void stop() override; virtual ~RpcLibServerBase() override; protected: - void* getServer(); - VehicleApiBase* getVehicleApi(); + void* getServer() const; + SimModeApiBase* getSimModeApi() const; private: - VehicleApiBase* vehicle_; + VehicleApiBase* getVehicleApi() const; + +private: + SimModeApiBase* simmode_api_; struct impl; std::unique_ptr pimpl_; }; diff --git a/AirLib/include/controllers/VehicleConnectorBase.hpp b/AirLib/include/controllers/VehicleConnectorBase.hpp index 0ea62eab2d..3b1fd07a8c 100644 --- a/AirLib/include/controllers/VehicleConnectorBase.hpp +++ b/AirLib/include/controllers/VehicleConnectorBase.hpp @@ -20,10 +20,6 @@ class VehicleConnectorBase : public UpdatableObject //called when render changes are required virtual void updateRendering(float dt) = 0; - //opens up channel to talk to vehicle via APIs - virtual void startApiServer() = 0; - virtual void stopApiServer() = 0; - virtual bool isApiServerStarted() = 0; virtual VehicleControllerBase* getController() = 0; virtual ImageCaptureBase* getImageCapture() = 0; virtual void setPose(const Pose& pose, bool ignore_collision) = 0; diff --git a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp index dc2f5a6f4c..3b6077dae3 100644 --- a/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp +++ b/AirLib/include/vehicles/car/api/CarRpcLibServer.hpp @@ -13,11 +13,11 @@ namespace msr { namespace airlib { class CarRpcLibServer : public RpcLibServerBase { public: - CarRpcLibServer(CarApiBase* vehicle, string server_address, uint16_t port = 42451); + CarRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port = 42451); virtual ~CarRpcLibServer(); private: - CarApiBase* getCarApi(); + CarApiBase* getCarApi() const; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp index 54e44e0b4f..e469d41f22 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp @@ -14,11 +14,11 @@ namespace msr { namespace airlib { class MultirotorRpcLibServer : public RpcLibServerBase { public: - MultirotorRpcLibServer(MultirotorApi* drone, string server_address, uint16_t port = 41451); + MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port = 41451); virtual ~MultirotorRpcLibServer(); private: - MultirotorApi* getDroneApi(); + MultirotorApi* getDroneApi() const; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp index 46c3a19c45..89af35c354 100644 --- a/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/RealMultirotorConnector.hpp @@ -26,19 +26,6 @@ class RealMultirotorConnector : public VehicleConnectorBase unused(dt); } - virtual void startApiServer() override - { - } - - virtual void stopApiServer() override - { - } - - virtual bool isApiServerStarted() override - { - return false; - } - virtual VehicleControllerBase* getController() override { return controller_; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 34a15d401e..1ee8237c81 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -45,8 +45,8 @@ struct RpcLibServerBase::impl { typedef msr::airlib_rpclib::RpcLibAdapatorsBase RpcLibAdapatorsBase; -RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_address, uint16_t port) - : vehicle_(vehicle) +RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_address, uint16_t port) + : simmode_api_(simmode_api) { if (server_address == "") pimpl_.reset(new impl(port)); @@ -57,11 +57,11 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres //sim only pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter) -> vector { - const auto& response = vehicle_->simGetImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); + const auto& response = getVehicleApi()->simGetImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); return RpcLibAdapatorsBase::ImageResponse::from(response); }); pimpl_->server.bind("simGetImage", [&](uint8_t camera_id, ImageCaptureBase::ImageType type) -> vector { - auto result = vehicle_->simGetImage(camera_id, type); + auto result = getVehicleApi()->simGetImage(camera_id, type); if (result.size() == 0) { // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. result.push_back(0); @@ -71,54 +71,54 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres pimpl_->server. bind("simSetPose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision) -> void { - vehicle_->simSetPose(pose.to(), ignore_collision); + getVehicleApi()->simSetPose(pose.to(), ignore_collision); }); pimpl_->server.bind("simGetPose", [&]() -> RpcLibAdapatorsBase::Pose { - const auto& pose = vehicle_->simGetPose(); + const auto& pose = getVehicleApi()->simGetPose(); return RpcLibAdapatorsBase::Pose(pose); }); pimpl_->server. bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool { - return vehicle_->simSetSegmentationObjectID(mesh_name, object_id, is_name_regex); + return getVehicleApi()->simSetSegmentationObjectID(mesh_name, object_id, is_name_regex); }); pimpl_->server. bind("simGetSegmentationObjectID", [&](const std::string& mesh_name) -> int { - return vehicle_->simGetSegmentationObjectID(mesh_name); + return getVehicleApi()->simGetSegmentationObjectID(mesh_name); }); pimpl_->server.bind("reset", [&]() -> void { - vehicle_->reset(); + getVehicleApi()->reset(); }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { - vehicle_->simPrintLogMessage(message, message_param, severity); + getVehicleApi()->simPrintLogMessage(message, message_param, severity); }); pimpl_->server.bind("getHomeGeoPoint", [&]() -> RpcLibAdapatorsBase::GeoPoint { - const auto& geo_point = vehicle_->getHomeGeoPoint(); + const auto& geo_point = getVehicleApi()->getHomeGeoPoint(); return RpcLibAdapatorsBase::GeoPoint(geo_point); }); pimpl_->server.bind("getCameraInfo", [&](int camera_id) -> RpcLibAdapatorsBase::CameraInfo { - const auto& camera_info = vehicle_->getCameraInfo(camera_id); + const auto& camera_info = getVehicleApi()->getCameraInfo(camera_id); return RpcLibAdapatorsBase::CameraInfo(camera_info); }); pimpl_->server.bind("setCameraOrientation", [&](int camera_id, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void { - vehicle_->setCameraOrientation(camera_id, orientation.to()); + getVehicleApi()->setCameraOrientation(camera_id, orientation.to()); }); - pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { vehicle_->enableApiControl(is_enabled); }); - pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return vehicle_->isApiControlEnabled(); }); + pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { getVehicleApi()->enableApiControl(is_enabled); }); + pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return getVehicleApi()->isApiControlEnabled(); }); pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { - const auto& collision_info = vehicle_->getCollisionInfo(); + const auto& collision_info = getVehicleApi()->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { - const auto& pose = vehicle_->simGetObjectPose(object_name); + const auto& pose = getVehicleApi()->simGetObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); @@ -129,7 +129,6 @@ RpcLibServerBase::RpcLibServerBase(VehicleApiBase* vehicle, string server_addres RpcLibServerBase::~RpcLibServerBase() { stop(); - vehicle_ = nullptr; } void RpcLibServerBase::start(bool block) @@ -145,14 +144,19 @@ void RpcLibServerBase::stop() pimpl_->server.stop(); } -void* RpcLibServerBase::getServer() +VehicleApiBase* RpcLibServerBase::getVehicleApi() const +{ + return simmode_api_->getVehicleApi(); +} + +void* RpcLibServerBase::getServer() const { return &pimpl_->server; } -VehicleApiBase* RpcLibServerBase::getVehicleApi() +SimModeApiBase* RpcLibServerBase::getSimModeApi() const { - return vehicle_; + return simmode_api_; } diff --git a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp index 0514e3df26..594e3d88b9 100644 --- a/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp +++ b/AirLib/src/vehicles/car/api/CarRpcLibServer.cpp @@ -30,8 +30,8 @@ namespace msr { namespace airlib { typedef msr::airlib_rpclib::CarRpcLibAdapators CarRpcLibAdapators; -CarRpcLibServer::CarRpcLibServer(CarApiBase* vehicle, string server_address, uint16_t port) - : RpcLibServerBase(vehicle, server_address, port) +CarRpcLibServer::CarRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) + : RpcLibServerBase(simmode_api, server_address, port) { (static_cast(getServer()))-> bind("getCarState", [&]() -> CarRpcLibAdapators::CarState { @@ -50,9 +50,9 @@ CarRpcLibServer::~CarRpcLibServer() { } -CarApiBase* CarRpcLibServer::getCarApi() +CarApiBase* CarRpcLibServer::getCarApi() const { - return static_cast(RpcLibServerBase::getVehicleApi()); + return static_cast(getSimModeApi()->getVehicleApi()); } diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 61525b3337..ca3bcba57f 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -30,8 +30,8 @@ namespace msr { namespace airlib { typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; -MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApi* drone, string server_address, uint16_t port) - : RpcLibServerBase(drone, server_address, port) +MultirotorRpcLibServer::MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) + : RpcLibServerBase(simmode_api, server_address, port) { (static_cast(getServer()))-> bind("armDisarm", [&](bool arm) -> bool { return getDroneApi()->armDisarm(arm); }); @@ -136,9 +136,9 @@ MultirotorRpcLibServer::~MultirotorRpcLibServer() { } -MultirotorApi* MultirotorRpcLibServer::getDroneApi() +MultirotorApi* MultirotorRpcLibServer::getDroneApi() const { - return static_cast(RpcLibServerBase::getVehicleApi()); + return static_cast(getSimModeApi()->getVehicleApi()); } }} //namespace diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index b435e15022..330af670dd 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -16,6 +16,22 @@ void printUsage() { cout << "Start the DroneServer using the 'PX4' settings in ~/Documents/AirSim/settings.json." << endl; } +class DroneServerSimModeApi : public SimModeApiBase { +public: + DroneServerSimModeApi(MultirotorApi* api) + : api_(api) + { + } + + virtual VehicleApiBase* getVehicleApi() override + { + return api_; + } + +private: + MultirotorApi* api_; +}; + int main(int argc, const char* argv[]) { if (argc != 2) { @@ -80,8 +96,9 @@ int main(int argc, const char* argv[]) RealMultirotorConnector connector(& mav_drone); - MultirotorApi server_wrapper(& connector); - msr::airlib::MultirotorRpcLibServer server(&server_wrapper, connection_info.local_host_ip); + MultirotorApi api(& connector); + DroneServerSimModeApi simmode_api(&api); + msr::airlib::MultirotorRpcLibServer server(&simmode_api, connection_info.local_host_ip); //start server in async mode server.start(false); @@ -92,7 +109,7 @@ int main(int argc, const char* argv[]) std::vector messages; while (true) { //check messages - server_wrapper.getStatusMessages(messages); + api.getStatusMessages(messages); if (messages.size() > 1) { for (const auto& message : messages) { std::cout << message << std::endl; diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 568a9ebb03..2e484b7fae 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - hello_drone.py + clock_speed.py . @@ -35,6 +35,9 @@ Code + + Code + Code diff --git a/PythonClient/clock_speed.py b/PythonClient/clock_speed.py new file mode 100644 index 0000000000..fb65582c3a --- /dev/null +++ b/PythonClient/clock_speed.py @@ -0,0 +1,21 @@ +from AirSimClient import * + + +# connect to the AirSim simulator +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +client.moveByVelocityZ(0, 0, -2, 3) + + +while True: + client.moveByVelocityZ(5, 5, -2, 1) + time.sleep(0.1) + +client.armDisarm(False) +client.reset() + +# that's enough fun for now. let's quit cleanly +client.enableApiControl(False) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 8d86c20cd3..ecc13e87dc 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -166,7 +166,7 @@ void ACarPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, HitNormal, NormalImpulse, Hit); } -void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_server_address, bool engine_sound) +void ACarPawn::initializeForBeginPlay(bool engine_sound) { if (engine_sound) EngineSoundComponent->Activate(); @@ -195,8 +195,8 @@ void ACarPawn::initializeForBeginPlay(bool enable_rpc, const std::string& api_se std::vector cameras = { InternalCamera1, InternalCamera2, InternalCamera3, InternalCamera4, InternalCamera5 }; wrapper_->initialize(this, cameras); wrapper_->setKinematics(&kinematics_); - - startApiServer(enable_rpc, api_server_address); + wrapper_->setApi(std::unique_ptr( + new CarPawnApi(wrapper_.get(), this->GetVehicleMovement()))); //TODO: should do reset() here? keyboard_controls_ = joystick_controls_ = CarPawnApi::CarControls(); @@ -228,44 +228,8 @@ msr::airlib::CarApiBase* ACarPawn::getApi() const return static_cast(wrapper_->getApi()); } -void ACarPawn::startApiServer(bool enable_rpc, const std::string& api_server_address) -{ - if (enable_rpc) { - wrapper_->setApi(std::unique_ptr( - new CarPawnApi(getVehiclePawnWrapper(), this->GetVehicleMovement()))); - -#ifdef AIRLIB_NO_RPC - rpclib_server_.reset(new msr::airlib::DebugApiServer()); -#else - rpclib_server_.reset(new msr::airlib::CarRpcLibServer(getApi(), api_server_address)); -#endif - - rpclib_server_->start(); - UAirBlueprintLib::LogMessageString("API server started at ", - api_server_address == "" ? "(default)" : api_server_address.c_str(), LogDebugLevel::Informational); - } - else - UAirBlueprintLib::LogMessageString("API server is disabled in settings", "", LogDebugLevel::Informational); - -} -void ACarPawn::stopApiServer() -{ - if (rpclib_server_ != nullptr) { - rpclib_server_->stop(); - rpclib_server_.reset(nullptr); - wrapper_->setApi(std::unique_ptr()); - } -} - -bool ACarPawn::isApiServerStarted() -{ - return rpclib_server_ != nullptr; -} - void ACarPawn::EndPlay(const EEndPlayReason::Type EndPlayReason) { - stopApiServer(); - if (InternalCamera1) InternalCamera1->DetachFromActor(FDetachmentTransformRules::KeepRelativeTransform); InternalCamera1 = nullptr; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index 2e714b6b14..fb10636acc 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -2,7 +2,6 @@ #include "CoreMinimal.h" #include "WheeledVehicle.h" -#include "vehicles/car/api/CarRpcLibServer.hpp" #include "physics/Kinematics.hpp" #include "CarPawnApi.h" #include "SimJoyStick/SimJoyStick.h" @@ -89,7 +88,7 @@ class ACarPawn : public AWheeledVehicle virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; VehiclePawnWrapper* getVehiclePawnWrapper(); - void initializeForBeginPlay(bool enable_rpc, const std::string& api_server_address, bool engine_sound); + void initializeForBeginPlay(bool engine_sound); virtual void NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) override; @@ -124,9 +123,6 @@ class ACarPawn : public AWheeledVehicle private: /** update the gear and speed strings */ void updateHUDStrings(); - void startApiServer(bool enable_rpc, const std::string& api_server_address); - void stopApiServer(); - bool isApiServerStarted(); void updateKinematics(float delta); void updateCarControls(); void updateForceFeedback(); @@ -135,13 +131,11 @@ class ACarPawn : public AWheeledVehicle void setupVehicleMovementComponent(); msr::airlib::CarApiBase* getApi() const; - private: typedef msr::airlib::AirSimSettings AirSimSettings; UClass* pip_camera_class_; - std::unique_ptr rpclib_server_; std::unique_ptr wrapper_; msr::airlib::Kinematics::State kinematics_; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index e5d0baf6bc..9048a6872c 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -4,6 +4,18 @@ #include "common/AirSimSettings.hpp" #include "Car/CarPawn.h" +#ifndef AIRLIB_NO_RPC + +#if defined _WIN32 || defined _WIN64 +#include "AllowWindowsPlatformTypes.h" +#endif +#include "vehicles/car/api/CarRpcLibServer.hpp" +#if defined _WIN32 || defined _WIN64 +#include "HideWindowsPlatformTypes.h" +#endif + +#endif + ASimModeCar::ASimModeCar() { static ConstructorHelpers::FClassFinder external_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); @@ -94,8 +106,7 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) //chose first pawn as FPV if none is designated as FPV VehiclePawnWrapper* wrapper = vehicle_pawn->getVehiclePawnWrapper(); - vehicle_pawn->initializeForBeginPlay(getSettings().enable_rpc, - getSettings().api_server_address, getSettings().engine_sound); + vehicle_pawn->initializeForBeginPlay(getSettings().engine_sound); if (getSettings().enable_collision_passthrough) wrapper->getConfig().enable_passthrough_on_collisions = true; @@ -132,6 +143,15 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) CameraDirector->initializeForBeginPlay(getInitialViewMode(), fpv_vehicle_pawn_wrapper_, external_camera); } +std::unique_ptr ASimModeCar::createApiServer() const +{ +#ifdef AIRLIB_NO_RPC + return ASimMode::createApiServer(); +#else + return std::unique_ptr(new msr::airlib::CarRpcLibServer( + getSimModeApi(), getSettings().api_server_address)); +#endif +} int ASimModeCar::getRemoteControlID(const VehiclePawnWrapper& pawn) const { diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 6baf3720d1..1c1a9be0e0 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -27,6 +27,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase void createVehicles(std::vector& vehicles); virtual void reset() override; virtual std::string getReport() override; + virtual std::unique_ptr createApiServer() const override; private: diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index 9545d28238..59b58b7b7f 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -1,47 +1,28 @@ #include "MultiRotorConnector.h" - -#ifdef AIRLIB_NO_RPC -#include "api/DebugApiServer.hpp" -#else - -#if defined _WIN32 || defined _WIN64 -#include "AllowWindowsPlatformTypes.h" -#endif -#include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp" -#if defined _WIN32 || defined _WIN64 -#include "HideWindowsPlatformTypes.h" -#endif - -#endif - #include "FlyingPawn.h" #include "AirBlueprintLib.h" #include using namespace msr::airlib; -MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* vehicle_pawn_wrapper, - msr::airlib::MultiRotorParams* vehicle_params, bool enable_rpc, - std::string api_server_address, uint16_t api_server_port, +MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* wrapper, + msr::airlib::MultiRotorParams* vehicle_params, UManualPoseController* manual_pose_controller) { - enable_rpc_ = enable_rpc; - api_server_address_ = api_server_address; - api_server_port_ = api_server_port; - vehicle_pawn_wrapper_ = vehicle_pawn_wrapper; + wrapper_ = wrapper; manual_pose_controller_ = manual_pose_controller; //reset roll & pitch of vehicle as multirotors required to be on plain surface at start - Pose pose = vehicle_pawn_wrapper->getPose(); + Pose pose = wrapper_->getPose(); float pitch, roll, yaw; VectorMath::toEulerianAngle(pose.orientation, pitch, roll, yaw); pose.orientation = VectorMath::toQuaternion(0, 0, yaw); - vehicle_pawn_wrapper->setPose(pose, false); + wrapper_->setPose(pose, false); vehicle_params_ = vehicle_params; - vehicle_.initialize(vehicle_params_, vehicle_pawn_wrapper_->getPose(), - vehicle_pawn_wrapper_->getHomePoint(), environment_); + vehicle_.initialize(vehicle_params_, wrapper_->getPose(), + wrapper_->getHomePoint(), environment_); controller_ = static_cast(vehicle_.getController()); @@ -56,6 +37,9 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* vehicle_pawn_wrappe reset_pending_ = false; did_reset_ = false; + wrapper_->setApi(std::unique_ptr( + new MultirotorApi(this))); + std::string message; if (!vehicle_.getController()->isAvailable(message)) { UAirBlueprintLib::LogMessage(FString("Vehicle was not initialized: "), FString(message.c_str()), LogDebugLevel::Failure); @@ -65,17 +49,16 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* vehicle_pawn_wrappe msr::airlib::ImageCaptureBase* MultiRotorConnector::getImageCapture() { - return vehicle_pawn_wrapper_->getImageCapture(); + return wrapper_->getImageCapture(); } Kinematics::State MultiRotorConnector::getTrueKinematics() { - return * vehicle_pawn_wrapper_->getTrueKinematics(); + return * wrapper_->getTrueKinematics(); } MultiRotorConnector::~MultiRotorConnector() { - stopApiServer(); } msr::airlib::VehicleControllerBase* MultiRotorConnector::getController() @@ -152,19 +135,19 @@ void MultiRotorConnector::updateRenderedState(float dt) } //move collision info from rendering engine to vehicle - const CollisionInfo& collision_info = vehicle_pawn_wrapper_->getCollisionInfo(); + const CollisionInfo& collision_info = wrapper_->getCollisionInfo(); vehicle_.setCollisionInfo(collision_info); //update ground level - if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == vehicle_pawn_wrapper_->getPawn()) { + if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == wrapper_->getPawn()) { FVector delta_position; FRotator delta_rotation; manual_pose_controller_->updateDeltaPosition(dt); manual_pose_controller_->getDeltaPose(delta_position, delta_rotation); manual_pose_controller_->resetDelta(); - Vector3r delta_position_ned = vehicle_pawn_wrapper_->getNedTransform().toNedMeters(delta_position, false); - Quaternionr delta_rotation_ned = vehicle_pawn_wrapper_->getNedTransform().toQuaternionr(delta_rotation.Quaternion(), true); + Vector3r delta_position_ned = wrapper_->getNedTransform().toNedMeters(delta_position, false); + Quaternionr delta_rotation_ned = wrapper_->getNedTransform().toQuaternionr(delta_rotation.Quaternion(), true); auto pose = vehicle_.getPose(); pose.position += delta_position_ned; @@ -222,19 +205,19 @@ void MultiRotorConnector::updateRendering(float dt) if (!VectorMath::hasNan(last_pose_)) { if (pending_pose_status_ == PendingPoseStatus::RenderPending) { - vehicle_pawn_wrapper_->setPose(last_pose_, pending_pose_collisions_); + wrapper_->setPose(last_pose_, pending_pose_collisions_); pending_pose_status_ = PendingPoseStatus::NonePending; } else - vehicle_pawn_wrapper_->setPose(last_pose_, false); + wrapper_->setPose(last_pose_, false); - vehicle_pawn_wrapper_->setDebugPose(last_debug_pose_); + wrapper_->setDebugPose(last_debug_pose_); } //update rotor animations for (unsigned int i = 0; i < rotor_count_; ++i) { RotorInfo* info = &rotor_info_[i]; - static_cast(vehicle_pawn_wrapper_->getPawn())-> + static_cast(wrapper_->getPawn())-> setRotorSpeed(i, info->rotor_speed * info->rotor_direction); } @@ -242,8 +225,8 @@ void MultiRotorConnector::updateRendering(float dt) UAirBlueprintLib::LogMessage(FString(controller_messages_[i].c_str()), TEXT(""), LogDebugLevel::Success, 30); } - if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == vehicle_pawn_wrapper_->getPawn()) { - UAirBlueprintLib::LogMessage(TEXT("Collision Count:"), FString::FromInt(vehicle_pawn_wrapper_->getCollisionInfo().collision_count), LogDebugLevel::Failure); + if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == wrapper_->getPawn()) { + UAirBlueprintLib::LogMessage(TEXT("Collision Count:"), FString::FromInt(wrapper_->getCollisionInfo().collision_count), LogDebugLevel::Failure); } else { //UAirBlueprintLib::LogMessage(TEXT("Collision (raw) Count:"), FString::FromInt(collision_response_info.collision_count_raw), LogDebugLevel::Unimportant); @@ -289,7 +272,7 @@ Pose MultiRotorConnector::getActorPose(const std::string& actor_name) msr::airlib::Pose pose; UAirBlueprintLib::RunCommandOnGameThread([&pose, &actor_name, this]() { - pose = vehicle_pawn_wrapper_->getActorPose(actor_name); + pose = wrapper_->getActorPose(actor_name); }, true); return pose; @@ -307,7 +290,7 @@ bool MultiRotorConnector::setSegmentationObjectID(const std::string& mesh_name, void MultiRotorConnector::printLogMessage(const std::string& message, std::string message_param, unsigned char severity) { - vehicle_pawn_wrapper_->printLogMessage(message, message_param, severity); + wrapper_->printLogMessage(message, message_param, severity); } int MultiRotorConnector::getSegmentationObjectID(const std::string& mesh_name) @@ -317,56 +300,21 @@ int MultiRotorConnector::getSegmentationObjectID(const std::string& mesh_name) CameraInfo MultiRotorConnector::getCameraInfo(int camera_id) const { - return vehicle_pawn_wrapper_->getCameraInfo(camera_id); + return wrapper_->getCameraInfo(camera_id); } void MultiRotorConnector::setCameraOrientation(int camera_id, const Quaternionr& orientation) { UAirBlueprintLib::RunCommandOnGameThread([&camera_id, &orientation, this]() { - vehicle_pawn_wrapper_->setCameraOrientation(camera_id, orientation); + wrapper_->setCameraOrientation(camera_id, orientation); }, true); } msr::airlib::MultirotorApi* MultiRotorConnector::getApi() const { - return static_cast(vehicle_pawn_wrapper_->getApi()); -} - -void MultiRotorConnector::startApiServer() -{ - if (enable_rpc_) { - vehicle_pawn_wrapper_->setApi( - std::unique_ptr(new msr::airlib::MultirotorApi(this))); - -#ifdef AIRLIB_NO_RPC - rpclib_server_.reset(new msr::airlib::DebugApiServer()); -#else - rpclib_server_.reset(new msr::airlib::MultirotorRpcLibServer( - getApi(), api_server_address_, api_server_port_)); -#endif - - rpclib_server_->start(); - UAirBlueprintLib::LogMessageString("API server started at ", - api_server_address_ == "" ? "(default)" : api_server_address_.c_str(), LogDebugLevel::Informational); - } - else - UAirBlueprintLib::LogMessageString("API server is disabled in settings", "", LogDebugLevel::Informational); - -} -void MultiRotorConnector::stopApiServer() -{ - if (rpclib_server_ != nullptr) { - getApi()->cancelAllTasks(); - rpclib_server_->stop(); - rpclib_server_.reset(nullptr); - vehicle_pawn_wrapper_->setApi(std::unique_ptr()); - } + return static_cast(wrapper_->getApi()); } -bool MultiRotorConnector::isApiServerStarted() -{ - return rpclib_server_ != nullptr; -} //*** Start: UpdatableState implementation ***// void MultiRotorConnector::reset() @@ -391,7 +339,7 @@ void MultiRotorConnector::resetPrivate() //controller_->reset(); rc_data_ = RCData(); - vehicle_pawn_wrapper_->reset(); //we do flier resetPose so that flier is placed back without collisions + wrapper_->reset(); //we do flier resetPose so that flier is placed back without collisions vehicle_.reset(); } @@ -406,8 +354,8 @@ void MultiRotorConnector::update() void MultiRotorConnector::reportState(StateReporter& reporter) { // report actual location in unreal coordinates so we can plug that into the UE editor to move the drone. - if (vehicle_pawn_wrapper_ != nullptr) { - FVector unrealPosition = vehicle_pawn_wrapper_->getUUPosition(); + if (wrapper_ != nullptr) { + FVector unrealPosition = wrapper_->getUUPosition(); reporter.writeValue("unreal pos", Vector3r(unrealPosition.X, unrealPosition.Y, unrealPosition.Z)); vehicle_.reportState(reporter); } diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h index 1efadfcfa9..b4e6f67eaa 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.h @@ -13,7 +13,7 @@ #include "controllers/VehicleConnectorBase.hpp" #include "ManualPoseController.h" #include -#include "api/ControlServerBase.hpp" +#include "api/ApiServerBase.hpp" #include "SimJoyStick/SimJoyStick.h" #include @@ -35,15 +35,11 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase //VehicleConnectorBase interface //implements game interface to update pawn - MultiRotorConnector(VehiclePawnWrapper* vehicle_paw_wrapper, msr::airlib::MultiRotorParams* vehicle_params, - bool enable_rpc, std::string api_server_address, uint16_t api_server_port, + MultiRotorConnector(VehiclePawnWrapper* vehicle_paw_wrapper, msr::airlib::MultiRotorParams* vehicle_params, UManualPoseController* manual_pose_controller); virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; - virtual void startApiServer() override; - virtual void stopApiServer() override; - virtual bool isApiServerStarted() override; virtual msr::airlib::VehicleControllerBase* getController() override; //PhysicsBody interface @@ -79,10 +75,9 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase MultiRotor vehicle_; std::vector controller_messages_; std::unique_ptr environment_; - VehiclePawnWrapper* vehicle_pawn_wrapper_; + VehiclePawnWrapper* wrapper_; msr::airlib::MultiRotorParams* vehicle_params_; - std::unique_ptr rpclib_server_; struct RotorInfo { real_T rotor_speed = 0; @@ -95,9 +90,6 @@ class MultiRotorConnector : public msr::airlib::VehicleConnectorBase CollisionResponseInfo collision_response_info; - bool enable_rpc_; - std::string api_server_address_; - uint16_t api_server_port_; msr::airlib::DroneControllerBase* controller_; UManualPoseController* manual_pose_controller_; diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index 30029c972b..4913602535 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -9,6 +9,18 @@ #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" #include "UnrealSensors/UnrealSensorFactory.h" +#ifndef AIRLIB_NO_RPC + +#if defined _WIN32 || defined _WIN64 +#include "AllowWindowsPlatformTypes.h" +#endif +#include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp" +#if defined _WIN32 || defined _WIN64 +#include "HideWindowsPlatformTypes.h" +#endif + +#endif + ASimModeWorldMultiRotor::ASimModeWorldMultiRotor() { @@ -21,17 +33,17 @@ ASimModeWorldMultiRotor::ASimModeWorldMultiRotor() void ASimModeWorldMultiRotor::BeginPlay() { Super::BeginPlay(); +} - //create control server - for (const std::shared_ptr& vehicle_connector_ : fpv_vehicle_connectors_) { - try { - vehicle_connector_->startApiServer(); - } - catch (std::exception& ex) { - UAirBlueprintLib::LogMessageString("Cannot start RpcLib Server", ex.what(), LogDebugLevel::Failure); - } - } +std::unique_ptr ASimModeWorldMultiRotor::createApiServer() const +{ +#ifdef AIRLIB_NO_RPC + return ASimMode::createApiServer(); +#else + return std::unique_ptr(new msr::airlib::MultirotorRpcLibServer( + getSimModeApi(), getSettings().api_server_address)); +#endif } void ASimModeWorldMultiRotor::EndPlay(const EEndPlayReason::Type EndPlayReason) @@ -39,9 +51,6 @@ void ASimModeWorldMultiRotor::EndPlay(const EEndPlayReason::Type EndPlayReason) //stop physics thread before we dismental stopAsyncUpdator(); - for (const std::shared_ptr& vehicle_connector_ : fpv_vehicle_connectors_) - vehicle_connector_->stopApiServer(); - //for (AActor* actor : spawned_actors_) { // actor->Destroy(); //} @@ -195,9 +204,7 @@ ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawn vehicle_params_.push_back(std::move(vehicle_params)); std::shared_ptr vehicle = std::make_shared( - wrapper, vehicle_params_.back().get(), getSettings().enable_rpc, getSettings().api_server_address, - vehicle_params_.back()->getParams().api_server_port + vehicle_params_.size() - 1, manual_pose_controller - ); + wrapper, vehicle_params_.back().get(), manual_pose_controller); if (vehicle->getPhysicsBody() != nullptr) wrapper->setKinematics(&(static_cast(vehicle->getPhysicsBody())->getKinematics())); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index b5fd2266f7..c49c8c2b72 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -22,6 +22,7 @@ class AIRSIM_API ASimModeWorldMultiRotor : public ASimModeWorldBase virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; VehiclePawnWrapper* getFpvVehiclePawnWrapper() const override; std::string getLogString() const; + virtual std::unique_ptr createApiServer() const override; protected: diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index fb57b4b523..de3e4c7b95 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -27,6 +27,7 @@ void ASimHUD::BeginPlay() createSimMode(); createMainWidget(); setupInputBindings(); + startApiServer(); } catch (std::exception& ex) { UAirBlueprintLib::LogMessageString("Error at startup: ", ex.what(), LogDebugLevel::Failure); @@ -44,6 +45,8 @@ void ASimHUD::Tick(float DeltaSeconds) void ASimHUD::EndPlay(const EEndPlayReason::Type EndPlayReason) { + stopApiServer(); + if (widget_) { widget_->Destruct(); widget_ = nullptr; @@ -74,6 +77,40 @@ void ASimHUD::inputEventToggleReport() widget_->setReportVisible(simmode_->EnableReport); } +void ASimHUD::startApiServer() +{ + if (AirSimSettings::singleton().enable_rpc) { + +#ifdef AIRLIB_NO_RPC + api_server_.reset(new msr::airlib::DebugApiServer()); +#else + api_server_ = std::move(simmode_->createApiServer()); +#endif + + try { + api_server_->start(); + } + catch (std::exception& ex) { + UAirBlueprintLib::LogMessageString("Cannot start RpcLib Server", ex.what(), LogDebugLevel::Failure); + } + } + else + UAirBlueprintLib::LogMessageString("API server is disabled in settings", "", LogDebugLevel::Informational); + +} +void ASimHUD::stopApiServer() +{ + if (api_server_ != nullptr) { + api_server_->stop(); + api_server_.reset(nullptr); + } +} + +bool ASimHUD::isApiServerStarted() +{ + return api_server_ != nullptr; +} + void ASimHUD::inputEventToggleHelp() { widget_->toggleHelpVisibility(); diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h index 16e0305289..379201421c 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h @@ -5,6 +5,8 @@ #include "SimHUDWidget.h" #include "SimMode/SimModeBase.h" #include "PIPCamera.h" +#include "api/ApiServerBase.hpp" +#include #include "SimHUD.generated.h" @@ -73,6 +75,10 @@ class AIRSIM_API ASimHUD : public AHUD std::string getSimModeFromUser(); + void startApiServer(); + void stopApiServer(); + bool isApiServerStarted(); + private: typedef common_utils::Utils Utils; UClass* widget_class_; @@ -81,6 +87,7 @@ class AIRSIM_API ASimHUD : public AHUD UPROPERTY() ASimModeBase* simmode_; APIPCamera* subwindow_cameras_[AirSimSettings::kSubwindowCount]; + std::unique_ptr api_server_; static ASimHUD* instance_; }; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp deleted file mode 100644 index 48332f770b..0000000000 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "SimModeApi.h" - -SimModeApi::SimModeApi(ASimModeBase* simmode) - : simmode_(simmode) -{ -} - -msr::airlib::VehicleApiBase* SimModeApi::getVehicleApi() -{ - return simmode_->getVehicleApi(); -} diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h deleted file mode 100644 index 541bcc279e..0000000000 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeApi.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "api/SimModeApiBase.hpp" -#include "SimMode/SimModeBase.h" - -class SimModeApi : msr::airlib::SimModeApiBase { -public: - SimModeApi(ASimModeBase* simmode); - virtual msr::airlib::VehicleApiBase* getVehicleApi() override; - -private: - ASimModeBase* simmode_; -}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index f604e8b933..ed60c3fa26 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -12,6 +12,7 @@ #include "Kismet/GameplayStatics.h" #include "SimJoyStick/SimJoyStick.h" #include "Misc/OutputDeviceNull.h" +#include "api/DebugApiServer.hpp" #include "common/EarthCelestial.hpp" @@ -31,6 +32,8 @@ void ASimModeBase::BeginPlay() { Super::BeginPlay(); + simmode_api_.reset(new SimModeApi(this)); + setupPhysicsLoopPeriod(); setupClockSpeed(); @@ -63,6 +66,11 @@ void ASimModeBase::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } +msr::airlib::SimModeApiBase* ASimModeBase::getSimModeApi() const +{ + return simmode_api_.get(); +} + void ASimModeBase::setupTimeOfDay() { @@ -111,6 +119,23 @@ msr::airlib::VehicleApiBase* ASimModeBase::getVehicleApi() const return nullptr; } +std::unique_ptr ASimModeBase::createApiServer() const +{ + //should be overriden by derived class + //by default we return no API server + return std::unique_ptr(new msr::airlib::DebugApiServer()); +} + +ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) + : simmode_(simmode) +{ +} + +msr::airlib::VehicleApiBase* ASimModeBase::SimModeApi::getVehicleApi() +{ + return simmode_->getVehicleApi(); +} + void ASimModeBase::setupClockSpeed() { //default setup - this should be overriden in derived modes as needed diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index f76a09b793..03f1dca38c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -10,6 +10,8 @@ #include "Components/SkyLightComponent.h" #include "common/ClockFactory.hpp" #include "Engine/DirectionalLight.h" +#include "api/ApiServerBase.hpp" +#include "api/SimModeApiBase.hpp" #include "SimModeBase.generated.h" @@ -49,6 +51,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual VehiclePawnWrapper* getFpvVehiclePawnWrapper() const; virtual msr::airlib::VehicleApiBase* getVehicleApi() const; + virtual std::unique_ptr createApiServer() const; protected: typedef msr::airlib::AirSimSettings AirSimSettings; @@ -56,7 +59,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual const AirSimSettings& getSettings() const; long long getPhysicsLoopPeriod() const; void setPhysicsLoopPeriod(long long period); - + msr::airlib::SimModeApiBase* getSimModeApi() const; virtual void setupClockSpeed(); protected: //settings @@ -69,6 +72,15 @@ class AIRSIM_API ASimModeBase : public AActor typedef msr::airlib::ClockFactory ClockFactory; typedef msr::airlib::TTimePoint TTimePoint; + class SimModeApi : public msr::airlib::SimModeApiBase { + public: + SimModeApi(ASimModeBase* simmode); + virtual msr::airlib::VehicleApiBase* getVehicleApi() override; + + private: + ASimModeBase* simmode_; + }; + private: UClass* sky_sphere_class_; UPROPERTY() AActor* sky_sphere_; @@ -77,6 +89,7 @@ class AIRSIM_API ASimModeBase : public AActor TTimePoint tod_last_update_; std::time_t tod_start_time_; long long physics_loop_period_; + std::unique_ptr simmode_api_; private: void setStencilIDs(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 45cb424cda..a34d0df68c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -8,7 +8,7 @@ #include "physics/World.hpp" #include "physics/PhysicsWorld.hpp" #include "common/StateReporterWrapper.hpp" -#include "api/ControlServerBase.hpp" +#include "api/ApiServerBase.hpp" #include "SimModeBase.h" #include "SimModeWorldBase.generated.h" From 2e1b66654bce7ed977ae72620c7f9f5656752947 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 24 Apr 2018 13:43:15 -0700 Subject: [PATCH 096/121] implemented pause and continueForTicks for multirotors --- .../common/common_utils/ScheduledExecutor.hpp | 70 +++++++++++++------ AirLib/include/physics/PhysicsWorld.hpp | 15 ++++ AirLib/include/physics/World.hpp | 15 ++++ PythonClient/clock_speed.py | 2 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 20 +++++- .../AirSim/Source/SimMode/SimModeBase.h | 4 ++ .../Source/SimMode/SimModeWorldBase.cpp | 16 +++++ .../AirSim/Source/SimMode/SimModeWorldBase.h | 4 ++ 8 files changed, 123 insertions(+), 23 deletions(-) diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp index 710cc1c211..8ab9b60252 100644 --- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp +++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp @@ -31,16 +31,17 @@ class ScheduledExecutor { callback_ = callback; period_nanos_ = period_nanos; started_ = false; - paused_ = false; + } void start() { started_ = true; - paused_ = false; + is_first_period_ = true; + initializePauseState(); + sleep_time_avg_ = 0; - period_count_ = 0; Utils::cleanupThread(th_); th_ = std::thread(&ScheduledExecutor::executorLoop, this); } @@ -55,11 +56,18 @@ class ScheduledExecutor { return paused_; } + void continueForTicks(uint32_t ticks) + { + pause_countdown_enabled_ = true; + pause_countdown_ = ticks; + paused_ = false; + } + void stop() { if (started_) { started_ = false; - paused_ = false; + initializePauseState(); try { if (th_.joinable()) { @@ -84,11 +92,6 @@ class ScheduledExecutor { return sleep_time_avg_; } - uint64_t getPeriodCount() const - { - return period_count_; - } - void lock() { mutex_.lock(); @@ -98,6 +101,14 @@ class ScheduledExecutor { mutex_.unlock(); } +private: + void initializePauseState() + { + paused_ = false; + pause_countdown_enabled_ = false; + pause_countdown_ = 0; + } + private: typedef std::chrono::high_resolution_clock clock; typedef uint64_t TTimePoint; @@ -140,16 +151,31 @@ class ScheduledExecutor { TTimePoint period_start = nanos(); TTimeDelta since_last_call = period_start - call_end; - //is this first loop? - if (period_count_ > 0 && !paused_) { - //when we are doing work, don't let other thread to cause contention - std::lock_guard locker(mutex_); - - bool result = callback_(since_last_call); - if (!result) { - started_ = result; + if (pause_countdown_enabled_) { + if (pause_countdown_ > 0) + --pause_countdown_; + else { + if (! paused_) + paused_ = true; + + pause_countdown_enabled_ = false; } } + + //is this first loop? + if (!is_first_period_) { + if (!paused_) { + //when we are doing work, don't let other thread to cause contention + std::lock_guard locker(mutex_); + + bool result = callback_(since_last_call); + if (!result) { + started_ = result; + } + } + } + else + is_first_period_ = false; call_end = nanos(); @@ -158,7 +184,6 @@ class ScheduledExecutor { TTimeDelta delay_nanos = period_nanos_ > elapsed_period ? period_nanos_ - elapsed_period : 0; //moving average of how much we are sleeping sleep_time_avg_ = 0.25f * sleep_time_avg_ + 0.75f * delay_nanos; - ++period_count_; if (delay_nanos > 0 && started_) sleep_for(delay_nanos); } @@ -168,10 +193,13 @@ class ScheduledExecutor { uint64_t period_nanos_; std::thread th_; std::function callback_; - std::atomic_bool started_, paused_; - + bool is_first_period_; + std::atomic_bool started_; + std::atomic_bool paused_; + std::atomic_uint32_t pause_countdown_; + std::atomic_bool pause_countdown_enabled_; + double sleep_time_avg_; - uint64_t period_count_; std::mutex mutex_; }; diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index 8b92744a85..9030b0677e 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -76,6 +76,21 @@ class PhysicsWorld { return reporter_.getOutput(); } + void pause(bool is_paused) + { + world_.pause(is_paused); + } + + bool isPaused() const + { + return world_.isPaused(); + } + + void continueForTicks(uint32_t ticks) + { + world_.continueForTicks(ticks); + } + private: void initializeWorld(const std::vector& bodies, bool start_async_updator) { diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index 142c13f7d3..ad3f302883 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -108,6 +108,21 @@ class World : public UpdatableContainer { executor_.stop(); } + void pause(bool is_paused) + { + executor_.pause(is_paused); + } + + bool isPaused() const + { + return executor_.isPaused(); + } + + void continueForTicks(uint32_t ticks) + { + executor_.continueForTicks(ticks); + } + private: bool worldUpdatorAsync(uint64_t dt_nanos) { diff --git a/PythonClient/clock_speed.py b/PythonClient/clock_speed.py index fb65582c3a..33a167a8cf 100644 --- a/PythonClient/clock_speed.py +++ b/PythonClient/clock_speed.py @@ -12,7 +12,7 @@ while True: client.moveByVelocityZ(5, 5, -2, 1) - time.sleep(0.1) + time.sleep(10) client.armDisarm(False) client.reset() diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index ed60c3fa26..7653be0bb4 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -119,10 +119,28 @@ msr::airlib::VehicleApiBase* ASimModeBase::getVehicleApi() const return nullptr; } +bool ASimModeBase::isPaused() const +{ + return false; +} + +void ASimModeBase::pause(bool is_paused) +{ + //should be overriden by derived class + unused(is_paused); + throw std::domain_error("Pause is not implemented by SimMode"); +} + +void ASimModeBase::continueForTicks(uint32_t ticks) +{ + //should be overriden by derived class + unused(ticks); + throw std::domain_error("continueForTicks is not implemented by SimMode"); +} + std::unique_ptr ASimModeBase::createApiServer() const { //should be overriden by derived class - //by default we return no API server return std::unique_ptr(new msr::airlib::DebugApiServer()); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 03f1dca38c..2047e7d385 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -53,6 +53,10 @@ class AIRSIM_API ASimModeBase : public AActor virtual std::unique_ptr createApiServer() const; + virtual bool isPaused() const; + virtual void pause(bool is_paused); + virtual void continueForTicks(uint32_t ticks); + protected: typedef msr::airlib::AirSimSettings AirSimSettings; virtual void setupInputBindings(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 283afdb481..8a3264553c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -87,6 +87,22 @@ ASimModeWorldBase::PhysicsEngineBase* ASimModeWorldBase::createPhysicsEngine() return physics_engine_.get(); } +bool ASimModeWorldBase::isPaused() const +{ + return physics_world_->isPaused(); +} + +void ASimModeWorldBase::pause(bool is_paused) +{ + physics_world_->pause(is_paused); +} + +void ASimModeWorldBase::continueForTicks(uint32_t ticks) +{ + physics_world_->continueForTicks(ticks); + +} + size_t ASimModeWorldBase::getVehicleCount() const { return vehicles_.size(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index a34d0df68c..8da23165a1 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -28,6 +28,10 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase virtual void reset() override; virtual std::string getReport() override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; + virtual void continueForTicks(uint32_t ticks) override; + protected: typedef std::shared_ptr VehiclePtr; virtual void createVehicles(std::vector& vehicles); From 953cb6c2ac4667436ab5cf3dd5ba1afdfaf60f4c Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 24 Apr 2018 16:22:57 -0700 Subject: [PATCH 097/121] pause-continue pattern --- AirLib/include/api/RpcLibClientBase.hpp | 4 + AirLib/include/api/SimModeApiBase.hpp | 3 + AirLib/src/api/RpcLibClientBase.cpp | 82 ++++++----- AirLib/src/api/RpcLibServerBase.cpp | 10 ++ PythonClient/AirSimClient.py | 128 +++++++++--------- PythonClient/PythonClient.pyproj | 8 +- PythonClient/pause_continue_car.py | 29 ++++ PythonClient/pause_continue_drone.py | 30 ++++ .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 5 + .../Plugins/AirSim/Source/AirBlueprintLib.h | 2 + .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 50 ++++++- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 8 ++ .../AirSim/Source/SimMode/SimModeBase.cpp | 38 ++++-- .../AirSim/Source/SimMode/SimModeBase.h | 3 + 14 files changed, 290 insertions(+), 110 deletions(-) create mode 100644 PythonClient/pause_continue_car.py create mode 100644 PythonClient/pause_continue_drone.py diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index c89f875ad9..93de43cd27 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -43,6 +43,10 @@ class RpcLibClientBase { CameraInfo getCameraInfo(int camera_id); void setCameraOrientation(int camera_id, const Quaternionr& orientation); + bool simIsPaused(); + void simPause(bool is_paused); + void simContinueForTicks(uint32_t ticks); + virtual ~RpcLibClientBase(); //required for pimpl protected: diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp index 35e8dfd91e..a1252b3a2e 100644 --- a/AirLib/include/api/SimModeApiBase.hpp +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -13,6 +13,9 @@ namespace msr { namespace airlib { class SimModeApiBase { public: virtual VehicleApiBase* getVehicleApi() = 0; + virtual bool isPaused() const = 0; + virtual void pause(bool is_paused) = 0; + virtual void continueForTicks(uint32_t ticks) = 0; virtual ~SimModeApiBase() = default; }; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 32b69ab75c..3e4f777418 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -85,39 +85,6 @@ bool RpcLibClientBase::isApiControlEnabled() return pimpl_->client.call("isApiControlEnabled").as(); } -//sim only -void RpcLibClientBase::simSetPose(const Pose& pose, bool ignore_collision) -{ - pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision); -} -Pose RpcLibClientBase::simGetPose() -{ - return pimpl_->client.call("simGetPose").as().to(); -} -vector RpcLibClientBase::simGetImages(vector request) -{ - const auto& response_adaptor = pimpl_->client.call("simGetImages", - RpcLibAdapatorsBase::ImageRequest::from(request)) - .as>(); - - return RpcLibAdapatorsBase::ImageResponse::to(response_adaptor); -} -vector RpcLibClientBase::simGetImage(int camera_id, ImageCaptureBase::ImageType type) -{ - vector result = pimpl_->client.call("simGetImage", camera_id, type).as>(); - if (result.size() == 1) { - // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. - result.clear(); - } - return result; -} - -void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) -{ - pimpl_->client.call("simPrintLogMessage", message, message_param, severity); -} - - msr::airlib::GeoPoint RpcLibClientBase::getHomeGeoPoint() { return pimpl_->client.call("getHomeGeoPoint").as().to(); @@ -167,6 +134,55 @@ CollisionInfo RpcLibClientBase::getCollisionInfo() return pimpl_->client.call("getCollisionInfo").as().to(); } + +//sim only +void RpcLibClientBase::simSetPose(const Pose& pose, bool ignore_collision) +{ + pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision); +} +Pose RpcLibClientBase::simGetPose() +{ + return pimpl_->client.call("simGetPose").as().to(); +} +vector RpcLibClientBase::simGetImages(vector request) +{ + const auto& response_adaptor = pimpl_->client.call("simGetImages", + RpcLibAdapatorsBase::ImageRequest::from(request)) + .as>(); + + return RpcLibAdapatorsBase::ImageResponse::to(response_adaptor); +} +vector RpcLibClientBase::simGetImage(int camera_id, ImageCaptureBase::ImageType type) +{ + vector result = pimpl_->client.call("simGetImage", camera_id, type).as>(); + if (result.size() == 1) { + // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead. + result.clear(); + } + return result; +} + +void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char severity) +{ + pimpl_->client.call("simPrintLogMessage", message, message_param, severity); +} + + +bool RpcLibClientBase::simIsPaused() +{ + return pimpl_->client.call("simIsPaused").as(); +} + +void RpcLibClientBase::simPause(bool is_paused) +{ + pimpl_->client.call("simPause", is_paused); +} + +void RpcLibClientBase::simContinueForTicks(uint32_t ticks) +{ + pimpl_->client.call("simContinueForTicks", ticks); +} + msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 1ee8237c81..212be6ba43 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -122,6 +122,16 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad return RpcLibAdapatorsBase::Pose(pose); }); + pimpl_->server.bind("simPause", [&](bool is_paused) -> void { + getSimModeApi()->pause(is_paused); + }); + pimpl_->server.bind("simIsPaused", [&]() -> bool { + return getSimModeApi()->isPaused(); + }); + pimpl_->server.bind("simContinueForTicks", [&](uint32_t ticks) -> void { + getSimModeApi()->continueForTicks(ticks); + }); + pimpl_->server.suppress_exceptions(true); } diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index fbdcbc00c7..97b6ea896c 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -213,67 +213,6 @@ class AirSimClientBase: def __init__(self, ip, port): self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = 3600, pack_encoding = 'utf-8', unpack_encoding = 'utf-8') - def ping(self): - return self.client.call('ping') - - def reset(self): - self.client.call('reset') - - def confirmConnection(self): - home = self.getHomeGeoPoint() - while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or - math.isnan(home.latitude) or math.isnan(home.longitude) or math.isnan(home.altitude)): - time.sleep(1) - home = self.getHomeGeoPoint() - print('X', end='') - print('') - - def getHomeGeoPoint(self): - return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint')) - - # basic flight control - def enableApiControl(self, is_enabled): - return self.client.call('enableApiControl', is_enabled) - def isApiControlEnabled(self): - return self.client.call('isApiControlEnabled') - - def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): - return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) - def simGetSegmentationObjectID(self, mesh_name): - return self.client.call('simGetSegmentationObjectID', mesh_name) - def simPrintLogMessage(self, message, message_param = "", severity = 0): - return self.client.call('simPrintLogMessage', message, message_param, severity) - def simGetObjectPose(self, object_name): - pose = self.client.call('simGetObjectPose', object_name) - return Pose.from_msgpack(pose) - - - # camera control - # simGetImage returns compressed png in array of bytes - # image_type uses one of the AirSimImageType members - def simGetImage(self, camera_id, image_type): - # because this method returns std::vector, msgpack decides to encode it as a string unfortunately. - result = self.client.call('simGetImage', camera_id, image_type) - if (result == "" or result == "\0"): - return None - return result - - # camera control - # simGetImage returns compressed png in array of bytes - # image_type uses one of the AirSimImageType members - def simGetImages(self, requests): - responses_raw = self.client.call('simGetImages', requests) - return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] - - def getCollisionInfo(self): - return CollisionInfo.from_msgpack(self.client.call('getCollisionInfo')) - - def getCameraInfo(self, camera_id): - return CameraInfo.from_msgpack(self.client.call('getCameraInfo', camera_id)) - - def setCameraOrientation(self, camera_id, orientation): - self.client.call('setCameraOrientation', camera_id, orientation) - @staticmethod def stringToUint8Array(bstr): return np.fromstring(bstr, np.uint8) @@ -496,6 +435,73 @@ def png_pack(png_tag, data): AirSimClientBase.write_file(filename, png_bytes) + def ping(self): + return self.client.call('ping') + + def reset(self): + self.client.call('reset') + + def confirmConnection(self): + home = self.getHomeGeoPoint() + while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or + math.isnan(home.latitude) or math.isnan(home.longitude) or math.isnan(home.altitude)): + time.sleep(1) + home = self.getHomeGeoPoint() + print('X', end='') + print('') + + def getHomeGeoPoint(self): + return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint')) + + # basic flight control + def enableApiControl(self, is_enabled): + return self.client.call('enableApiControl', is_enabled) + def isApiControlEnabled(self): + return self.client.call('isApiControlEnabled') + + def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): + return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) + def simGetSegmentationObjectID(self, mesh_name): + return self.client.call('simGetSegmentationObjectID', mesh_name) + def simPrintLogMessage(self, message, message_param = "", severity = 0): + return self.client.call('simPrintLogMessage', message, message_param, severity) + def simGetObjectPose(self, object_name): + pose = self.client.call('simGetObjectPose', object_name) + return Pose.from_msgpack(pose) + + + # camera control + # simGetImage returns compressed png in array of bytes + # image_type uses one of the AirSimImageType members + def simGetImage(self, camera_id, image_type): + # because this method returns std::vector, msgpack decides to encode it as a string unfortunately. + result = self.client.call('simGetImage', camera_id, image_type) + if (result == "" or result == "\0"): + return None + return result + + # camera control + # simGetImage returns compressed png in array of bytes + # image_type uses one of the AirSimImageType members + def simGetImages(self, requests): + responses_raw = self.client.call('simGetImages', requests) + return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] + + def getCollisionInfo(self): + return CollisionInfo.from_msgpack(self.client.call('getCollisionInfo')) + + def getCameraInfo(self, camera_id): + return CameraInfo.from_msgpack(self.client.call('getCameraInfo', camera_id)) + + def setCameraOrientation(self, camera_id, orientation): + self.client.call('setCameraOrientation', camera_id, orientation) + + def simIsPause(self): + return self.client.call("simIsPaused") + def simPause(self, is_paused): + self.client.call('simPause', is_paused) + def simContinueForTicks(self, ticks): + self.client.call('simContinueForTicks', int(ticks)) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(AirSimClientBase, object): diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 2e484b7fae..653d5b1142 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - clock_speed.py + pause_continue_car.py . @@ -35,6 +35,9 @@ Code + + Code + Code @@ -56,6 +59,9 @@ Code + + Code + Code diff --git a/PythonClient/pause_continue_car.py b/PythonClient/pause_continue_car.py new file mode 100644 index 0000000000..312f3f93e0 --- /dev/null +++ b/PythonClient/pause_continue_car.py @@ -0,0 +1,29 @@ +from AirSimClient import * + +# connect to the AirSim simulator +client = CarClient() +client.confirmConnection() +client.enableApiControl(True) + +car_controls = CarControls() + +for i in range(1, 6): + print("Starting command") + car_controls.throttle = 0.5 + car_controls.steering = 1 + client.setCarControls(car_controls) + time.sleep(5) #run + print("Pausing after 5sec") + client.simPause(True) + time.sleep(5) #paused + print("Restarting command to run for 10sec") + client.simContinueForTicks(600) #600 ticks of 1/60sec each = 10sec + time.sleep(20) + print("Finishing rest of the command") + client.simPause(False) + time.sleep(10) + print("Finished cycle") + + + + diff --git a/PythonClient/pause_continue_drone.py b/PythonClient/pause_continue_drone.py new file mode 100644 index 0000000000..8abaf49264 --- /dev/null +++ b/PythonClient/pause_continue_drone.py @@ -0,0 +1,30 @@ +from AirSimClient import * + +# connect to the AirSim simulator +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +print("Taking off") +client.moveByVelocityZ(0, 0, -20, 8) +time.sleep(3) + +for i in range(1, 6): + print("Starting command to run for 15sec") + client.moveByVelocityZ(-1*i, -1*i, -20-i, 15) + time.sleep(5) #run + print("Pausing after 5sec") + client.simPause(True) + time.sleep(5) #paused + print("Restarting command to run for 6sec") + client.simContinueForTicks(2000) #2000 ticks of 3ms each = 1.5sec + time.sleep(10) + print("Finishing rest of the command") + client.simPause(False) + time.sleep(10) + print("Finished cycle") + + + + diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 1f3f1f1cf8..be52c306b7 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -185,6 +185,11 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, //GEngine->AddOnScreenDebugMessage(key + 10, 60.0f, color, FString::FromInt(key)); } +void UAirBlueprintLib::setUnrealClockSpeed(const AActor* context, float clock_speed) +{ + context->GetWorldSettings()->SetTimeDilation(clock_speed); +} + float UAirBlueprintLib::GetWorldToMetersScale(const AActor* context) { float w2m = 100.f; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 2d1788b867..3ecb5af9da 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -111,6 +111,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static UObject* LoadObject(const std::string& name); static UClass* LoadClass(const std::string& name); + static void setUnrealClockSpeed(const AActor* context, float clock_speed); + private: template static void InitializeObjectStencilID(T* obj, bool ignore_existing = true); diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 9048a6872c..2249d1bd63 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -30,6 +30,8 @@ void ASimModeCar::BeginPlay() { Super::BeginPlay(); + initializePauseState(); + createVehicles(vehicles_); report_wrapper_.initialize(false); @@ -52,15 +54,42 @@ VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() const return fpv_vehicle_pawn_wrapper_; } +void ASimModeCar::initializePauseState() +{ + pause_countdown_ = 0; + pause_countdown_enabled_ = false; + pause(false); +} + +bool ASimModeCar::isPaused() const +{ + return current_clockspeed_ == 0; +} + +void ASimModeCar::pause(bool is_paused) +{ + if (is_paused) + current_clockspeed_ = 0; + else + current_clockspeed_ = getSettings().clock_speed; + + UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_); +} + +void ASimModeCar::continueForTicks(uint32_t ticks) +{ + pause_countdown_enabled_ = true; + pause_countdown_ = ticks; + pause(false); +} + void ASimModeCar::setupClockSpeed() { - float clock_speed = getSettings().clock_speed; + current_clockspeed_ = getSettings().clock_speed; //setup clock in PhysX - if (clock_speed != 1.0f) { - this->GetWorldSettings()->SetTimeDilation(clock_speed); - UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(clock_speed), LogDebugLevel::Informational); - } + UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_); + UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(current_clockspeed_), LogDebugLevel::Informational); } void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) @@ -193,6 +222,17 @@ void ASimModeCar::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); + if (pause_countdown_enabled_) { + if (pause_countdown_ > 0) + --pause_countdown_; + else { + if (!isPaused()) + pause(true); + + pause_countdown_enabled_ = false; + } + } + report_wrapper_.update(); report_wrapper_.setEnable(EnableReport); diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 1c1a9be0e0..c931b3cdf2 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -29,11 +29,15 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual std::string getReport() override; virtual std::unique_ptr createApiServer() const override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; + virtual void continueForTicks(uint32_t ticks) override; private: void setupVehiclesAndCamera(std::vector& vehicles); void updateReport(); int getRemoteControlID(const VehiclePawnWrapper& pawn) const; + void initializePauseState(); protected: virtual void setupClockSpeed() override; @@ -48,4 +52,8 @@ class AIRSIM_API ASimModeCar : public ASimModeBase VehiclePawnWrapper* fpv_vehicle_pawn_wrapper_; float follow_distance_; msr::airlib::StateReporterWrapper report_wrapper_; + + float current_clockspeed_; + uint32_t pause_countdown_; + bool pause_countdown_enabled_; }; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 7653be0bb4..0b1b461637 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -144,16 +144,6 @@ std::unique_ptr ASimModeBase::createApiServer() cons return std::unique_ptr(new msr::airlib::DebugApiServer()); } -ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) - : simmode_(simmode) -{ -} - -msr::airlib::VehicleApiBase* ASimModeBase::SimModeApi::getVehicleApi() -{ - return simmode_->getVehicleApi(); -} - void ASimModeBase::setupClockSpeed() { //default setup - this should be overriden in derived modes as needed @@ -318,3 +308,31 @@ void ASimModeBase::stopRecording() FRecordingThread::stopRecording(); } +//************************* SimModeApi *****************************/ + +ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) + : simmode_(simmode) +{ +} + +msr::airlib::VehicleApiBase* ASimModeBase::SimModeApi::getVehicleApi() +{ + return simmode_->getVehicleApi(); +} + +bool ASimModeBase::SimModeApi::isPaused() const +{ + return simmode_->isPaused(); +} + +void ASimModeBase::SimModeApi::pause(bool is_paused) +{ + simmode_->pause(is_paused); +} + +void ASimModeBase::SimModeApi::continueForTicks(uint32_t ticks) +{ + simmode_->continueForTicks(ticks); +} + +//************************* SimModeApi *****************************/ diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 2047e7d385..e61c0f6bbe 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -80,6 +80,9 @@ class AIRSIM_API ASimModeBase : public AActor public: SimModeApi(ASimModeBase* simmode); virtual msr::airlib::VehicleApiBase* getVehicleApi() override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; + virtual void continueForTicks(uint32_t ticks) override; private: ASimModeBase* simmode_; From 9a1f6a2b7d2dcb0f3196f0eb4839610a1a5ec0a3 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 24 Apr 2018 18:55:36 -0700 Subject: [PATCH 098/121] change continueForTicks to continueForTime, docs --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/SimModeApiBase.hpp | 2 +- .../common/common_utils/ScheduledExecutor.hpp | 28 +++++++++---------- AirLib/include/physics/PhysicsWorld.hpp | 4 +-- AirLib/include/physics/World.hpp | 4 +-- AirLib/src/api/RpcLibClientBase.cpp | 4 +-- AirLib/src/api/RpcLibServerBase.cpp | 4 +-- PythonClient/AirSimClient.py | 4 +-- PythonClient/PythonClient.pyproj | 2 +- PythonClient/pause_continue_car.py | 2 +- PythonClient/pause_continue_drone.py | 6 ++-- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 18 ++++++------ Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 13 ++++++--- .../AirSim/Source/SimMode/SimModeBase.cpp | 10 +++---- .../AirSim/Source/SimMode/SimModeBase.h | 6 ++-- .../Source/SimMode/SimModeWorldBase.cpp | 4 +-- .../AirSim/Source/SimMode/SimModeWorldBase.h | 2 +- docs/apis.md | 11 +++++--- 18 files changed, 66 insertions(+), 60 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 93de43cd27..ab082bd54c 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -45,7 +45,7 @@ class RpcLibClientBase { bool simIsPaused(); void simPause(bool is_paused); - void simContinueForTicks(uint32_t ticks); + void simContinueForTime(double seconds); virtual ~RpcLibClientBase(); //required for pimpl diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp index a1252b3a2e..6a6d4b3fc8 100644 --- a/AirLib/include/api/SimModeApiBase.hpp +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -15,7 +15,7 @@ class SimModeApiBase { virtual VehicleApiBase* getVehicleApi() = 0; virtual bool isPaused() const = 0; virtual void pause(bool is_paused) = 0; - virtual void continueForTicks(uint32_t ticks) = 0; + virtual void continueForTime(double seconds) = 0; virtual ~SimModeApiBase() = default; }; diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp index 8ab9b60252..108146c5ec 100644 --- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp +++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp @@ -56,10 +56,10 @@ class ScheduledExecutor { return paused_; } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - pause_countdown_enabled_ = true; - pause_countdown_ = ticks; + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); paused_ = false; } @@ -105,8 +105,8 @@ class ScheduledExecutor { void initializePauseState() { paused_ = false; - pause_countdown_enabled_ = false; - pause_countdown_ = 0; + pause_period_start_ = 0; + pause_period_ = 0; } private: @@ -151,14 +151,12 @@ class ScheduledExecutor { TTimePoint period_start = nanos(); TTimeDelta since_last_call = period_start - call_end; - if (pause_countdown_enabled_) { - if (pause_countdown_ > 0) - --pause_countdown_; - else { - if (! paused_) - paused_ = true; - - pause_countdown_enabled_ = false; + if (pause_period_start_ > 0) { + if (nanos() - pause_period_start_ >= pause_period_) { + if (! isPaused()) + pause(true); + + pause_period_start_ = 0; } } @@ -196,8 +194,8 @@ class ScheduledExecutor { bool is_first_period_; std::atomic_bool started_; std::atomic_bool paused_; - std::atomic_uint32_t pause_countdown_; - std::atomic_bool pause_countdown_enabled_; + std::atomic pause_period_; + std::atomic pause_period_start_; double sleep_time_avg_; diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index 9030b0677e..f397b5ed26 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -86,9 +86,9 @@ class PhysicsWorld { return world_.isPaused(); } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - world_.continueForTicks(ticks); + world_.continueForTime(seconds); } private: diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index ad3f302883..5d9d03df64 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -118,9 +118,9 @@ class World : public UpdatableContainer { return executor_.isPaused(); } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - executor_.continueForTicks(ticks); + executor_.continueForTime(seconds); } private: diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 3e4f777418..442e6fb62f 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -178,9 +178,9 @@ void RpcLibClientBase::simPause(bool is_paused) pimpl_->client.call("simPause", is_paused); } -void RpcLibClientBase::simContinueForTicks(uint32_t ticks) +void RpcLibClientBase::simContinueForTime(double seconds) { - pimpl_->client.call("simContinueForTicks", ticks); + pimpl_->client.call("simContinueForTime", seconds); } msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 212be6ba43..6d208bcc00 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -128,8 +128,8 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad pimpl_->server.bind("simIsPaused", [&]() -> bool { return getSimModeApi()->isPaused(); }); - pimpl_->server.bind("simContinueForTicks", [&](uint32_t ticks) -> void { - getSimModeApi()->continueForTicks(ticks); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { + getSimModeApi()->continueForTime(seconds); }); pimpl_->server.suppress_exceptions(true); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 97b6ea896c..00b76f84ef 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -500,8 +500,8 @@ def simIsPause(self): return self.client.call("simIsPaused") def simPause(self, is_paused): self.client.call('simPause', is_paused) - def simContinueForTicks(self, ticks): - self.client.call('simContinueForTicks', int(ticks)) + def simContinueForTime(self, seconds): + self.client.call('simContinueForTime', seconds) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(AirSimClientBase, object): diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 653d5b1142..f659f7df69 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - pause_continue_car.py + hello_car.py . diff --git a/PythonClient/pause_continue_car.py b/PythonClient/pause_continue_car.py index 312f3f93e0..c6abd7ca29 100644 --- a/PythonClient/pause_continue_car.py +++ b/PythonClient/pause_continue_car.py @@ -17,7 +17,7 @@ client.simPause(True) time.sleep(5) #paused print("Restarting command to run for 10sec") - client.simContinueForTicks(600) #600 ticks of 1/60sec each = 10sec + client.simContinueForTime(10) time.sleep(20) print("Finishing rest of the command") client.simPause(False) diff --git a/PythonClient/pause_continue_drone.py b/PythonClient/pause_continue_drone.py index 8abaf49264..de93168ffc 100644 --- a/PythonClient/pause_continue_drone.py +++ b/PythonClient/pause_continue_drone.py @@ -17,12 +17,12 @@ print("Pausing after 5sec") client.simPause(True) time.sleep(5) #paused - print("Restarting command to run for 6sec") - client.simContinueForTicks(2000) #2000 ticks of 3ms each = 1.5sec + print("Restarting command to run for 7.5sec") + client.simContinueForTime(7.5) time.sleep(10) print("Finishing rest of the command") client.simPause(False) - time.sleep(10) + time.sleep(15) print("Finished cycle") diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 2249d1bd63..c25528c349 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -56,8 +56,8 @@ VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() const void ASimModeCar::initializePauseState() { - pause_countdown_ = 0; - pause_countdown_enabled_ = false; + pause_period_ = 0; + pause_period_start_ = 0; pause(false); } @@ -76,10 +76,10 @@ void ASimModeCar::pause(bool is_paused) UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_); } -void ASimModeCar::continueForTicks(uint32_t ticks) +void ASimModeCar::continueForTime(double seconds) { - pause_countdown_enabled_ = true; - pause_countdown_ = ticks; + pause_period_start_ = ClockFactory::get()->nowNanos(); + pause_period_ = seconds; pause(false); } @@ -222,14 +222,12 @@ void ASimModeCar::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); - if (pause_countdown_enabled_) { - if (pause_countdown_ > 0) - --pause_countdown_; - else { + if (pause_period_start_ > 0) { + if (ClockFactory::get()->elapsedSince(pause_period_start_) >= pause_period_) { if (!isPaused()) pause(true); - pause_countdown_enabled_ = false; + pause_period_start_ = 0; } } diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index c931b3cdf2..2ce4c3ee9c 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -31,7 +31,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; private: void setupVehiclesAndCamera(std::vector& vehicles); @@ -44,6 +44,11 @@ class AIRSIM_API ASimModeCar : public ASimModeBase private: + typedef msr::airlib::ClockFactory ClockFactory; + typedef common_utils::Utils Utils; + typedef msr::airlib::TTimePoint TTimePoint; + typedef msr::airlib::TTimeDelta TTimeDelta; + UClass* external_camera_class_; UClass* camera_director_class_; @@ -53,7 +58,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase float follow_distance_; msr::airlib::StateReporterWrapper report_wrapper_; - float current_clockspeed_; - uint32_t pause_countdown_; - bool pause_countdown_enabled_; + std::atomic current_clockspeed_; + std::atomic pause_period_; + std::atomic pause_period_start_; }; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 0b1b461637..cde38fa5e7 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -131,11 +131,11 @@ void ASimModeBase::pause(bool is_paused) throw std::domain_error("Pause is not implemented by SimMode"); } -void ASimModeBase::continueForTicks(uint32_t ticks) +void ASimModeBase::continueForTime(double seconds) { //should be overriden by derived class - unused(ticks); - throw std::domain_error("continueForTicks is not implemented by SimMode"); + unused(seconds); + throw std::domain_error("continueForTime is not implemented by SimMode"); } std::unique_ptr ASimModeBase::createApiServer() const @@ -330,9 +330,9 @@ void ASimModeBase::SimModeApi::pause(bool is_paused) simmode_->pause(is_paused); } -void ASimModeBase::SimModeApi::continueForTicks(uint32_t ticks) +void ASimModeBase::SimModeApi::continueForTime(double seconds) { - simmode_->continueForTicks(ticks); + simmode_->continueForTime(seconds); } //************************* SimModeApi *****************************/ diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index e61c0f6bbe..123ddca578 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -55,7 +55,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual bool isPaused() const; virtual void pause(bool is_paused); - virtual void continueForTicks(uint32_t ticks); + virtual void continueForTime(double seconds); protected: typedef msr::airlib::AirSimSettings AirSimSettings; @@ -75,6 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor typedef common_utils::Utils Utils; typedef msr::airlib::ClockFactory ClockFactory; typedef msr::airlib::TTimePoint TTimePoint; + typedef msr::airlib::TTimeDelta TTimeDelta; + class SimModeApi : public msr::airlib::SimModeApiBase { public: @@ -82,7 +84,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual msr::airlib::VehicleApiBase* getVehicleApi() override; virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; private: ASimModeBase* simmode_; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 8a3264553c..1115cb963f 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -97,9 +97,9 @@ void ASimModeWorldBase::pause(bool is_paused) physics_world_->pause(is_paused); } -void ASimModeWorldBase::continueForTicks(uint32_t ticks) +void ASimModeWorldBase::continueForTime(double seconds) { - physics_world_->continueForTicks(ticks); + physics_world_->continueForTime(seconds); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 8da23165a1..2f2daa2c0e 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -30,7 +30,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; protected: typedef std::shared_ptr VehiclePtr; diff --git a/docs/apis.md b/docs/apis.md index 49e41d0e59..ebd8284f44 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -93,14 +93,17 @@ You can find a ready to run project in HelloCar folder in the repository. * `simPrintLogMessage`: Prints the specified message in the simulator's window. If message_param is also supplied then its printed next to the message and in that case if this API is called with same message value but different message_param again then previous line is overwritten with new line (instead of API creating new line on display). For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API is called with different values of i. The valid values of severity parameter is 0 to 3 inclusive that corresponds to different colors. * `simGetObjectPose`: Gets the pose of specified object in Unreal environment. Here the object means "actor" in Unreal terminology. They are searched by tag as well as name. To add tag to actor, go to object in Unreal Editor, click on it, look for [Tags property](https://answers.unrealengine.com/questions/543807/whats-the-difference-between-tag-and-tag.html), click "+" sign and add some string value. If multiple actors have same tag then the first match is returned. If no matches are found then NaN pose is returned. -### Coordinate System -All AirSim API uses NED coordinate system, i.e., +X is North, +Y is East and +Z is Down. All units are in SI system. Please note that this is different from coordinate system used internally by Unreal Engine. In Unreal Engine, +Z is up instead of down and length unit is in centimeters instead of meters. AirSim APIs takes care of the appropriate conversions. The starting point of the vehicle is always coordinates (0, 0, 0) in NED system. Thus when converting from Unreal coordinates to NED, we first subtract the starting offset and then scale by 100 for cm to m conversion. - -## Image / Computer Vision and Collision APIs +### Image / Computer Vision and Collision APIs AirSim offers comprehensive images APIs to retrieve synchronized images from multiple cameras along with ground truth including depth, disparity, surface normals and vision. You can set the resolution, FOV, motion blur etc parameters in [settings.json](settings.md). There is also API for detecting collision state. See also [complete code](../Examples/StereoImageGenerator.hpp) that generates specified number of stereo images and ground truth depth with normalization to camera plan, computation of disparity image and saving it to [pfm format](pfm.md). More on [image APIs and Computer Vision mode](image_apis.md). +### Pause and Continue APIs +AirSim allows to pause and continue the simulation through `pause(is_paused)` API. To pause the simulation call `pause(True)` and to continue the simulation call `pause(False)`. You may have scenario, especially while runninf reinforcement learning, to run the simulation for specified amount of time and then automatically pause. While simulation is paused, you may then do some expensive computation, issue a new command and then again run the simulation for specified amount of time. This can be achieved by another API `continueForTime(seconds)`. This API runs the simulation for the specified number of seconds and then pauses the simulation. For example usage, please see [pause_continue_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_car.py) and [pause_continue_drone.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_drone.py). + +### Coordinate System +All AirSim API uses NED coordinate system, i.e., +X is North, +Y is East and +Z is Down. All units are in SI system. Please note that this is different from coordinate system used internally by Unreal Engine. In Unreal Engine, +Z is up instead of down and length unit is in centimeters instead of meters. AirSim APIs takes care of the appropriate conversions. The starting point of the vehicle is always coordinates (0, 0, 0) in NED system. Thus when converting from Unreal coordinates to NED, we first subtract the starting offset and then scale by 100 for cm to m conversion. + ## Vehicle Specific APIs ### APIs for Car Car has followings APIs available: From 46fd4935cc40ff6d98ef51c17807449ec7927caf Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Tue, 24 Apr 2018 19:03:23 -0700 Subject: [PATCH 099/121] Minor pause and continue doc fixes --- docs/apis.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/apis.md b/docs/apis.md index ebd8284f44..d7e0c3ce5c 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -99,7 +99,7 @@ AirSim offers comprehensive images APIs to retrieve synchronized images from mul More on [image APIs and Computer Vision mode](image_apis.md). ### Pause and Continue APIs -AirSim allows to pause and continue the simulation through `pause(is_paused)` API. To pause the simulation call `pause(True)` and to continue the simulation call `pause(False)`. You may have scenario, especially while runninf reinforcement learning, to run the simulation for specified amount of time and then automatically pause. While simulation is paused, you may then do some expensive computation, issue a new command and then again run the simulation for specified amount of time. This can be achieved by another API `continueForTime(seconds)`. This API runs the simulation for the specified number of seconds and then pauses the simulation. For example usage, please see [pause_continue_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_car.py) and [pause_continue_drone.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_drone.py). +AirSim allows to pause and continue the simulation through `pause(is_paused)` API. To pause the simulation call `pause(True)` and to continue the simulation call `pause(False)`. You may have scenario, especially while using reinforcement learning, to run the simulation for specified amount of time and then automatically pause. While simulation is paused, you may then do some expensive computation, send a new command and then again run the simulation for specified amount of time. This can be achieved by API `continueForTime(seconds)`. This API runs the simulation for the specified number of seconds and then pauses the simulation. For example usage, please see [pause_continue_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_car.py) and [pause_continue_drone.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_drone.py). ### Coordinate System All AirSim API uses NED coordinate system, i.e., +X is North, +Y is East and +Z is Down. All units are in SI system. Please note that this is different from coordinate system used internally by Unreal Engine. In Unreal Engine, +Z is up instead of down and length unit is in centimeters instead of meters. AirSim APIs takes care of the appropriate conversions. The starting point of the vehicle is always coordinates (0, 0, 0) in NED system. Thus when converting from Unreal coordinates to NED, we first subtract the starting offset and then scale by 100 for cm to m conversion. @@ -154,4 +154,4 @@ Adding new APIs requires modifying the source code. Much of the changes are mech * AirSim APIs using [Python](python.md) * [move on path](https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo) demo showing video of fast multirotor flight through Modular Neighborhood environment * [building a hexacopter](https://github.com/Microsoft/AirSim/wiki/hexacopter) -* [building point clouds](https://github.com/Microsoft/AirSim/wiki/Point-Clouds) \ No newline at end of file +* [building point clouds](https://github.com/Microsoft/AirSim/wiki/Point-Clouds) From 00de800568be9a93bacb35d267e47abd87f245af Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 25 Apr 2018 13:37:01 -0700 Subject: [PATCH 100/121] fix compile issue in DroneServer --- DroneServer/main.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index 330af670dd..b2d1ed150d 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -28,6 +28,21 @@ class DroneServerSimModeApi : public SimModeApiBase { return api_; } + virtual bool isPaused() const override + { + return false; + } + + virtual void pause(bool is_paused) override + { + throw std::domain_error("pause is not supported"); + } + + virtual void continueForTime(double seconds) override + { + throw std::domain_error("continueForTime is not supported"); + } + private: MultirotorApi* api_; }; From 1e735df6ae09157d4db85521fa85d582d2097408 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Wed, 25 Apr 2018 14:24:15 -0700 Subject: [PATCH 101/121] removed unused environments --- build_all_ue_projects.bat | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/build_all_ue_projects.bat b/build_all_ue_projects.bat index 7c45a3fa4e..8fa9addb87 100644 --- a/build_all_ue_projects.bat +++ b/build_all_ue_projects.bat @@ -32,10 +32,10 @@ IF NOT EXIST "%OutputPath%" mkdir "%OutputPath%" call:doOneProject "CityEnviron" call:doOneProject "ZhangJiaJie" call:doOneProject "AirSimEnvNH" -call:doOneProject "AncientRome" -call:doOneProject "DowntownCar" +REM call:doOneProject "AncientRome" +REM call:doOneProject "DowntownCar" call:doOneProject "LandscapeMountains" -call:doOneProject "ModularCity" +REM call:doOneProject "ModularCity" call:doOneProject "Africa_001" "Africa" goto :done From 43ff7ce8586ba52fd2ecc47f0774625cba965993 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 26 Apr 2018 14:14:29 -0700 Subject: [PATCH 102/121] fixed part of clang compile error: https://github.com/Microsoft/AirSim/issues/1023 --- AirLib/include/vehicles/car/api/CarApiBase.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index a80d9670cb..345107d370 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -59,8 +59,8 @@ class CarApiBase : public VehicleApiBase { CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, bool handbrake_val, const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val, uint64_t timestamp_val) - : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val), - rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val) + : speed(speed_val), gear(gear_val), rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val), collision(collision_val), + kinematics_true(kinematics_true_val), timestamp(timestamp_val) { } }; From f96fb66802f79be40847bc93c1940859114bc9d4 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 26 Apr 2018 15:25:30 -0700 Subject: [PATCH 103/121] Fix linux compile errors: https://github.com/Microsoft/AirSim/issues/1023 --- AirLib/include/api/RpcLibServerBase.hpp | 2 +- Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 2 +- .../AirSim/Source/SimJoyStick/SimJoyStick.cpp | 13 +++++++++++++ docs/build_linux.md | 11 +++++++++++ 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/AirLib/include/api/RpcLibServerBase.hpp b/AirLib/include/api/RpcLibServerBase.hpp index 4dd279e72c..fcae3ed06c 100644 --- a/AirLib/include/api/RpcLibServerBase.hpp +++ b/AirLib/include/api/RpcLibServerBase.hpp @@ -6,7 +6,7 @@ #include "common/Common.hpp" #include "api/ApiServerBase.hpp" -#include "api/SimModeAPiBase.hpp" +#include "api/SimModeApiBase.hpp" namespace msr { namespace airlib { diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index de3e4c7b95..e82def7fbe 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -84,7 +84,7 @@ void ASimHUD::startApiServer() #ifdef AIRLIB_NO_RPC api_server_.reset(new msr::airlib::DebugApiServer()); #else - api_server_ = std::move(simmode_->createApiServer()); + api_server_ = simmode_->createApiServer(); #endif try { diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp index 20b637b0cd..1cfed61205 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp @@ -317,6 +317,19 @@ struct SimJoyStick::impl { state.is_valid = false; } + + void setAutoCenter(unsigned int index, double strength) { + unused(index); + unused(strength); + //TODO: implement this for linux + } + + void setWheelRumble(unsigned int index, double strength) { + unused(index); + unused(strength); + //TODO: implement this for linux + } + // bool getJoystickInfo(int index, std::string& manufacturerID, std::string& productID, std::string& message) // { // manufacturerID = productID = ""; diff --git a/docs/build_linux.md b/docs/build_linux.md index 8f06338402..b11500e8db 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -39,6 +39,17 @@ Finally, you will need an Unreal project that hosts the environment for your veh ## FAQ +#### I'm getting error " could not be compiled. Try rebuilding from source manually". + +This could either happen because of compile error or the fact that your gch files are outdated. Look in to your console window. Do you see something like below? +``` +fatal errorfatal error: : file '/usr/include/linux/version.h''/usr/include/linux/version.h' has been modified since the precompiled header +``` + +If this is the case then look for *.gch file(s) that follows after that message, delete them and try again. Here's [relevant thread](https://answers.unrealengine.com/questions/412349/linux-ue4-build-precompiled-header-fatal-error.html) on Unreal Engine forums. + +If you see other compile errors in console then open up those source files and see if it is due to changes you made. If not, then report it as issue on GitHub. + #### What are the known issues with Unreal 4.16? * One of the major issues is [this bug in Unreal](https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html). We have a workaround for some parts of the code but we haven't tested if everything is covered. From 4ff8992b0bd1c4b31c63d98bff2d430c982e3e94 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 26 Apr 2018 19:42:35 -0700 Subject: [PATCH 104/121] fix bug in orbit.py if you are not taking snapshots. --- PythonClient/orbit.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/orbit.py b/PythonClient/orbit.py index 9d97a3b822..11aea9a12e 100644 --- a/PythonClient/orbit.py +++ b/PythonClient/orbit.py @@ -24,7 +24,7 @@ def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center self.snapshot_index = 0 self.takeoff = False # whether we did a take off - if self.snapshots > 0: + if self.snapshots is not None and self.snapshots > 0: self.snapshot_delta = 360 / self.snapshots if self.iterations <= 0: From 4fc9aaaa2c76363f33842a48473b41ff8e8a464e Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Thu, 26 Apr 2018 19:44:22 -0700 Subject: [PATCH 105/121] add helper script to simply query current position. --- PythonClient/getpos.py | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 PythonClient/getpos.py diff --git a/PythonClient/getpos.py b/PythonClient/getpos.py new file mode 100644 index 0000000000..c98e412b11 --- /dev/null +++ b/PythonClient/getpos.py @@ -0,0 +1,11 @@ +from AirSimClient import * + +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) + +pos = client.getPosition() +print("x={}, y={}, z={}".format(pos.x_val, pos.y_val, pos.z_val)) + +pos = client.getPitchRollYaw() +print("pitch={}, roll={}, yaw={}".format(pos[0], pos[1], pos[2])) \ No newline at end of file From e6fb56ea5d8a911f5870ed846823ec36eed828c6 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Thu, 26 Apr 2018 22:56:42 -0700 Subject: [PATCH 106/121] unify reset() mechanism with consistency across car and drone, make armDisarm as promitive --- AirLib/include/api/RpcLibClientBase.hpp | 1 + AirLib/include/api/SimModeApiBase.hpp | 1 + AirLib/include/api/VehicleApiBase.hpp | 1 + .../vehicles/multirotor/api/MultirotorApi.hpp | 2 +- .../controllers/DroneControllerBase.hpp | 2 +- .../simple_flight/firmware/OffboardApi.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 5 + AirLib/src/api/RpcLibServerBase.cpp | 4 +- .../multirotor/api/MultirotorRpcLibClient.cpp | 4 - .../multirotor/api/MultirotorRpcLibServer.cpp | 2 - DroneServer/main.cpp | 5 + MavLinkCom/include/MavLinkConnection.hpp | 8 +- MavLinkCom/src/MavLinkConnection.cpp | 50 ++++---- MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 12 +- MavLinkCom/src/impl/MavLinkConnectionImpl.hpp | 116 +++++++++--------- PythonClient/AirSimClient.py | 6 +- PythonClient/PythonClient.pyproj | 7 ++ PythonClient/reset_test_car.py | 25 ++++ PythonClient/reset_test_drone.py | 23 ++++ Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 18 +-- Unreal/Plugins/AirSim/Source/Car/CarPawn.h | 2 - .../Plugins/AirSim/Source/Car/CarPawnApi.cpp | 9 +- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h | 1 + .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 18 +-- .../Source/Multirotor/MultiRotorConnector.cpp | 13 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 7 +- .../AirSim/Source/SimMode/SimModeBase.h | 1 + .../Source/SimMode/SimModeWorldBase.cpp | 5 +- .../AirSim/Source/VehiclePawnWrapper.cpp | 9 +- docs/apis.md | 2 +- docs/reinforcement_learning.md | 2 + 31 files changed, 211 insertions(+), 152 deletions(-) create mode 100644 PythonClient/reset_test_car.py create mode 100644 PythonClient/reset_test_drone.py diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index ab082bd54c..7cd0e5aad7 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -32,6 +32,7 @@ class RpcLibClientBase { bool isApiControlEnabled(); void enableApiControl(bool is_enabled); void reset(); + bool armDisarm(bool arm); CollisionInfo getCollisionInfo(); diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp index 6a6d4b3fc8..b095733903 100644 --- a/AirLib/include/api/SimModeApiBase.hpp +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -14,6 +14,7 @@ class SimModeApiBase { public: virtual VehicleApiBase* getVehicleApi() = 0; virtual bool isPaused() const = 0; + virtual void reset() = 0; virtual void pause(bool is_paused) = 0; virtual void continueForTime(double seconds) = 0; virtual ~SimModeApiBase() = default; diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index 6eac26639d..5bb5656589 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -14,6 +14,7 @@ class VehicleApiBase { public: virtual GeoPoint getHomeGeoPoint() const = 0; virtual void enableApiControl(bool is_enabled) = 0; + virtual bool armDisarm(bool arm) = 0; virtual bool isApiControlEnabled() const = 0; virtual void reset() = 0; diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index 829133c155..ea47702522 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -44,7 +44,7 @@ class MultirotorApi : public VehicleApiBase { } virtual ~MultirotorApi() = default; - bool armDisarm(bool arm) + virtual bool armDisarm(bool arm) override { CallLock lock(controller_, action_mutex_, cancel_mutex_, pending_); pending_ = std::make_shared(); diff --git a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp index 66c7a879e0..26d4e37857 100644 --- a/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp +++ b/AirLib/include/vehicles/multirotor/controllers/DroneControllerBase.hpp @@ -45,7 +45,7 @@ class DroneControllerBase : public VehicleControllerBase { public: const SafetyEval::EvalResult result; - UnsafeMoveException(const SafetyEval::EvalResult result_val, const string message = "") + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") : VehicleMoveException(message), result(result_val) {} }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index 5018983ab9..d81f6ffaf9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -44,7 +44,7 @@ class OffboardApi : else { if (clock_->millis() - goal_timestamp_ > params_->api_goal_timeout) { if (!is_api_timedout_) { - comm_link_->log("API call timed out, entering hover mode"); + comm_link_->log("API call was not received, entering hover mode for safety"); goal_mode_ = GoalMode::getPositionMode(); goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true); is_api_timedout_ = true; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 442e6fb62f..a31b124a26 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -119,6 +119,11 @@ void* RpcLibClientBase::getClient() return &pimpl_->client; } +bool RpcLibClientBase::armDisarm(bool arm) +{ + return pimpl_->client.call("armDisarm", arm).as(); +} + CameraInfo RpcLibClientBase::getCameraInfo(int camera_id) { return pimpl_->client.call("getCameraInfo", camera_id).as().to(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 6d208bcc00..fff57ca485 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -88,7 +88,7 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad }); pimpl_->server.bind("reset", [&]() -> void { - getVehicleApi()->reset(); + getSimModeApi()->reset(); }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { @@ -111,7 +111,7 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad pimpl_->server.bind("enableApiControl", [&](bool is_enabled) -> void { getVehicleApi()->enableApiControl(is_enabled); }); pimpl_->server.bind("isApiControlEnabled", [&]() -> bool { return getVehicleApi()->isApiControlEnabled(); }); - + pimpl_->server.bind("armDisarm", [&](bool arm) -> bool { return getVehicleApi()->armDisarm(arm); }); pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo { const auto& collision_info = getVehicleApi()->getCollisionInfo(); return RpcLibAdapatorsBase::CollisionInfo(collision_info); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 46df2b0c89..10b29cb8c5 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -40,10 +40,6 @@ MultirotorRpcLibClient::MultirotorRpcLibClient(const string& ip_address, uint16 MultirotorRpcLibClient::~MultirotorRpcLibClient() {} -bool MultirotorRpcLibClient::armDisarm(bool arm) -{ - return static_cast(getClient())->call("armDisarm", arm).as(); -} void MultirotorRpcLibClient::setSimulationMode(bool is_set) { static_cast(getClient())->call("setSimulationMode", is_set); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index ca3bcba57f..b9b869628b 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -33,8 +33,6 @@ typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators; MultirotorRpcLibServer::MultirotorRpcLibServer(SimModeApiBase* simmode_api, string server_address, uint16_t port) : RpcLibServerBase(simmode_api, server_address, port) { - (static_cast(getServer()))-> - bind("armDisarm", [&](bool arm) -> bool { return getDroneApi()->armDisarm(arm); }); (static_cast(getServer()))-> bind("setSimulationMode", [&](bool is_set) -> void { getDroneApi()->setSimulationMode(is_set); }); (static_cast(getServer()))-> diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp index b2d1ed150d..ce5812aa00 100644 --- a/DroneServer/main.cpp +++ b/DroneServer/main.cpp @@ -28,6 +28,11 @@ class DroneServerSimModeApi : public SimModeApiBase { return api_; } + virtual void reset() override + { + throw std::domain_error("reset is not supported"); + } + virtual bool isPaused() const override { return false; diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index ba22d8d02c..c1b270bd10 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/MavLinkCom/include/MavLinkConnection.hpp @@ -60,7 +60,7 @@ namespace mavlinkcom { // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5"). // pass initial string to write to the port, which can be used to configure the port. // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams. - static std::shared_ptr connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = ""); + static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = ""); // Start listening on a specific local port for packets from any remote computer. Once a packet is received // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender. @@ -69,7 +69,7 @@ namespace mavlinkcom { // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet // adapter rather than 127.0.0.1. - static std::shared_ptr connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort); + static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); // Connect to a specific remote machine that is already listening on a specific port for messages from any computer. // This will use any free local port that is available. @@ -77,13 +77,13 @@ namespace mavlinkcom { // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet // adapter rather than 127.0.0.1. - static std::shared_ptr connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort); + static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); // This method sets up a tcp connection to the specified remote host and port. The remote host // must already be listening and accepting TCP socket connections for this to succeed. // The localAddr can also a specific local ip address if you need to specify which // NIC to use, for example, wifi versus hard wired ethernet adapter. For localhost pass 127.0.0.1. - static std::shared_ptr connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort); + static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); // instance methods std::string getName(); diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 6c2d321e1a..2150e4a967 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -9,49 +9,49 @@ using namespace mavlinkcom_impl; MavLinkConnection::MavLinkConnection() { - pImpl.reset(new MavLinkConnectionImpl()); + pImpl.reset(new MavLinkConnectionImpl()); } -std::shared_ptr MavLinkConnection::connectSerial(const std::string& nodeName, std::string portName, int baudrate, const std::string initString) +std::shared_ptr MavLinkConnection::connectSerial(const std::string& nodeName, const std::string& portName, int baudrate, const std::string& initString) { - return MavLinkConnectionImpl::connectSerial(nodeName, portName, baudrate, initString); + return MavLinkConnectionImpl::connectSerial(nodeName, portName, baudrate, initString); } -std::shared_ptr MavLinkConnection::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort) +std::shared_ptr MavLinkConnection::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort) { - return MavLinkConnectionImpl::connectLocalUdp(nodeName, localAddr, localPort); + return MavLinkConnectionImpl::connectLocalUdp(nodeName, localAddr, localPort); } -std::shared_ptr MavLinkConnection::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort) +std::shared_ptr MavLinkConnection::connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort) { - return MavLinkConnectionImpl::connectRemoteUdp(nodeName, localAddr, remoteAddr, remotePort); + return MavLinkConnectionImpl::connectRemoteUdp(nodeName, localAddr, remoteAddr, remotePort); } -std::shared_ptr MavLinkConnection::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort) +std::shared_ptr MavLinkConnection::connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort) { - return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); + return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort); } void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr connectedPort) { - pImpl->startListening(shared_from_this(), nodeName, connectedPort); + pImpl->startListening(shared_from_this(), nodeName, connectedPort); } // log every message that is "sent" using sendMessage. void MavLinkConnection::startLoggingSendMessage(std::shared_ptr log) { - pImpl->startLoggingSendMessage(log); + pImpl->startLoggingSendMessage(log); } void MavLinkConnection::stopLoggingSendMessage() { - pImpl->stopLoggingSendMessage(); + pImpl->stopLoggingSendMessage(); } void MavLinkConnection::close() { - pImpl->close(); + pImpl->close(); } bool MavLinkConnection::isOpen() @@ -70,54 +70,54 @@ int MavLinkConnection::prepareForSending(MavLinkMessage& msg) std::string MavLinkConnection::getName() { - return pImpl->getName(); + return pImpl->getName(); } int MavLinkConnection::getTargetComponentId() { - return pImpl->getTargetComponentId(); + return pImpl->getTargetComponentId(); } int MavLinkConnection::getTargetSystemId() { - return pImpl->getTargetSystemId(); + return pImpl->getTargetSystemId(); } uint8_t MavLinkConnection::getNextSequence() { - return pImpl->getNextSequence(); + return pImpl->getNextSequence(); } void MavLinkConnection::sendMessage(const MavLinkMessageBase& msg) { - pImpl->sendMessage(msg); + pImpl->sendMessage(msg); } void MavLinkConnection::sendMessage(const MavLinkMessage& msg) { - pImpl->sendMessage(msg); + pImpl->sendMessage(msg); } int MavLinkConnection::subscribe(MessageHandler handler) { - return pImpl->subscribe(handler); + return pImpl->subscribe(handler); } void MavLinkConnection::unsubscribe(int id) { - pImpl->unsubscribe(id); + pImpl->unsubscribe(id); } MavLinkConnection::~MavLinkConnection() { - pImpl->close(); - pImpl = nullptr; + pImpl->close(); + pImpl = nullptr; } void MavLinkConnection::join(std::shared_ptr remote, bool subscribeToLeft, bool subscribeToRight) { - pImpl->join(remote, subscribeToLeft, subscribeToRight); + pImpl->join(remote, subscribeToLeft, subscribeToRight); } // get the next telemetry snapshot, then clear the internal counters and start over. This way each snapshot // gives you a picture of what happened in whatever timeslice you decide to call this method. void MavLinkConnection::getTelemetry(MavLinkTelemetry& result) { - pImpl->getTelemetry(result); + pImpl->getTelemetry(result); } diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index f52b3d25d4..f38d5e914b 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -45,7 +45,7 @@ std::shared_ptr MavLinkConnectionImpl::createConnection(cons return con; } -std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort) +std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort) { std::shared_ptr socket = std::make_shared(); @@ -54,7 +54,7 @@ std::shared_ptr MavLinkConnectionImpl::connectLocalUdp(const return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort) +std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort) { std::string local = localAddr; // just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also. @@ -69,7 +69,7 @@ std::shared_ptr MavLinkConnectionImpl::connectRemoteUdp(cons return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort) +std::shared_ptr MavLinkConnectionImpl::connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort) { std::string local = localAddr; // just a little sanity check on the local address, if remoteAddr is localhost then localAddr must be also. @@ -84,13 +84,13 @@ std::shared_ptr MavLinkConnectionImpl::connectTcp(const std: return createConnection(nodeName, socket); } -std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, std::string name, int baudRate, const std::string initString) +std::shared_ptr MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString) { std::shared_ptr serial = std::make_shared(); - int hr = serial->connect(name.c_str(), baudRate); + int hr = serial->connect(portName.c_str(), baudRate); if (hr != 0) - throw std::runtime_error(Utils::stringf("Could not open the serial port %s, error=%d", name.c_str(), hr)); + throw std::runtime_error(Utils::stringf("Could not open the serial port %s, error=%d", portName.c_str(), hr)); // send this right away just in case serial link is not already configured if (initString.size() > 0) { diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index a8d9e53e68..daa83dfc85 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp @@ -27,77 +27,77 @@ using namespace mavlinkcom; namespace mavlinkcom_impl { - // See MavLinkConnection.hpp for definitions of these methods. - class MavLinkConnectionImpl - { - public: - MavLinkConnectionImpl(); - static std::shared_ptr connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = ""); - static std::shared_ptr connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort); - static std::shared_ptr connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort); - static std::shared_ptr connectTcp(const std::string& nodeName, std::string localAddr, const std::string& remoteIpAddr, int remotePort); + // See MavLinkConnection.hpp for definitions of these methods. + class MavLinkConnectionImpl + { + public: + MavLinkConnectionImpl(); + static std::shared_ptr connectSerial(const std::string& nodeName, const std::string& portName, int baudRate = 115200, const std::string& initString = ""); + static std::shared_ptr connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort); + static std::shared_ptr connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort); + static std::shared_ptr connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort); - std::string getName(); - int getTargetComponentId(); - int getTargetSystemId(); - ~MavLinkConnectionImpl(); - void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); - void startLoggingSendMessage(std::shared_ptr log); - void stopLoggingSendMessage(); - void close(); + std::string getName(); + int getTargetComponentId(); + int getTargetSystemId(); + ~MavLinkConnectionImpl(); + void startListening(std::shared_ptr parent, const std::string& nodeName, std::shared_ptr connectedPort); + void startLoggingSendMessage(std::shared_ptr log); + void stopLoggingSendMessage(); + void close(); bool isOpen(); - void sendMessage(const MavLinkMessageBase& msg); - void sendMessage(const MavLinkMessage& msg); - int subscribe(MessageHandler handler); - void unsubscribe(int id); - uint8_t getNextSequence(); - void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); - void getTelemetry(MavLinkTelemetry& result); + void sendMessage(const MavLinkMessageBase& msg); + void sendMessage(const MavLinkMessage& msg); + int subscribe(MessageHandler handler); + void unsubscribe(int id); + uint8_t getNextSequence(); + void join(std::shared_ptr remote, bool subscribeToLeft = true, bool subscribeToRight = true); + void getTelemetry(MavLinkTelemetry& result); void ignoreMessage(uint8_t message_id); int prepareForSending(MavLinkMessage& msg); - private: - static std::shared_ptr createConnection(const std::string& nodeName, std::shared_ptr port); + private: + static std::shared_ptr createConnection(const std::string& nodeName, std::shared_ptr port); void joinLeftSubscriber(std::shared_ptr remote, std::shared_ptrcon, const MavLinkMessage& msg); void joinRightSubscriber(std::shared_ptrcon, const MavLinkMessage& msg); - void publishPackets(); - void readPackets(); - void drainQueue(); - std::string name; - std::shared_ptr port; - std::shared_ptr con_; - int other_system_id = -1; - int other_component_id = 0; - uint8_t next_seq = 0; - std::thread read_thread; - std::string accept_node_name_; - std::shared_ptr server_; - std::shared_ptr sendLog_; + void publishPackets(); + void readPackets(); + void drainQueue(); + std::string name; + std::shared_ptr port; + std::shared_ptr con_; + int other_system_id = -1; + int other_component_id = 0; + uint8_t next_seq = 0; + std::thread read_thread; + std::string accept_node_name_; + std::shared_ptr server_; + std::shared_ptr sendLog_; - struct MessageHandlerEntry { - public: - int id; - MessageHandler handler; - }; - std::vector listeners; - std::vector snapshot; - bool snapshot_stale; - std::mutex listener_mutex; - uint8_t message_buf[300]; // must be bigger than sizeof(mavlink_message_t), which is currently 292. - std::mutex buffer_mutex; - bool closed; - std::thread publish_thread_; - std::queue msg_queue_; - std::mutex msg_queue_mutex_; - mavlink_utils::Semaphore msg_available_; - bool waiting_for_msg_ = false; + struct MessageHandlerEntry { + public: + int id; + MessageHandler handler; + }; + std::vector listeners; + std::vector snapshot; + bool snapshot_stale; + std::mutex listener_mutex; + uint8_t message_buf[300]; // must be bigger than sizeof(mavlink_message_t), which is currently 292. + std::mutex buffer_mutex; + bool closed; + std::thread publish_thread_; + std::queue msg_queue_; + std::mutex msg_queue_mutex_; + mavlink_utils::Semaphore msg_available_; + bool waiting_for_msg_ = false; bool supports_mavlink2_ = false; bool signing_ = false; mavlink_status_t mavlink_intermediate_status_; mavlink_status_t mavlink_status_; std::mutex telemetry_mutex_; - MavLinkTelemetry telemetry_; + MavLinkTelemetry telemetry_; std::unordered_set ignored_messageids; - }; + }; } #endif diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 00b76f84ef..4b73733a2e 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -453,6 +453,9 @@ def confirmConnection(self): def getHomeGeoPoint(self): return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint')) + def armDisarm(self, arm): + return self.client.call('armDisarm', arm) + # basic flight control def enableApiControl(self, is_enabled): return self.client.call('enableApiControl', is_enabled) @@ -510,9 +513,6 @@ def __init__(self, ip = ""): ip = "127.0.0.1" super(MultirotorClient, self).__init__(ip, 41451) - def armDisarm(self, arm): - return self.client.call('armDisarm', arm) - def takeoff(self, max_wait_seconds = 15): return self.client.call('takeoff', max_wait_seconds) diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index f659f7df69..defc1d5090 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -35,6 +35,9 @@ Code + + + Code @@ -63,6 +66,10 @@ Code + + + Code + Code diff --git a/PythonClient/reset_test_car.py b/PythonClient/reset_test_car.py new file mode 100644 index 0000000000..d2d1c30b18 --- /dev/null +++ b/PythonClient/reset_test_car.py @@ -0,0 +1,25 @@ +from AirSimClient import * + +# connect to the AirSim simulator +client = CarClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) +car_controls = CarControls() + +# go forward +car_controls.throttle = 1 +car_controls.steering = 1 +client.setCarControls(car_controls) +print("Go Foward") +time.sleep(5) # let car drive a bit + +print("reset") +client.reset() +time.sleep(5) # let car drive a bit + +client.setCarControls(car_controls) +print("Go Foward") +time.sleep(5) # let car drive a bit + + diff --git a/PythonClient/reset_test_drone.py b/PythonClient/reset_test_drone.py new file mode 100644 index 0000000000..b1358b9939 --- /dev/null +++ b/PythonClient/reset_test_drone.py @@ -0,0 +1,23 @@ +from AirSimClient import * + + +# connect to the AirSim simulator +client = MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +print("fly") +client.moveToPosition(0, 0, -10, 5) +time.sleep(5) # let car drive a bit + +print("reset") +client.reset() +client.enableApiControl(True) +client.armDisarm(True) +time.sleep(5) # let car drive a bit + + +print("fly") +client.moveToPosition(0, 0, -10, 5) +time.sleep(5) # let car drive a bit \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index ecc13e87dc..209ae7fcb7 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -214,15 +214,6 @@ void ACarPawn::initializeForBeginPlay(bool engine_sound) } -void ACarPawn::reset(bool disable_api_control) -{ - keyboard_controls_ = joystick_controls_ = CarPawnApi::CarControls(); - wrapper_->setApi(std::unique_ptr()); - - if (disable_api_control) - getApi()->enableApiControl(false); -} - msr::airlib::CarApiBase* ACarPawn::getApi() const { return static_cast(wrapper_->getApi()); @@ -378,10 +369,11 @@ void ACarPawn::updateCarControls() if (wrapper_->getRemoteControlID() >= 0 && joystick_state_.is_initialized) { joystick_.getJoyStickState(0, joystick_state_); - if ((joystick_state_.buttons & 4) | (joystick_state_.buttons & 1024)) { //X button or Start button - reset(); - return; - } + //TODO: move this to SimModeBase + //if ((joystick_state_.buttons & 4) | (joystick_state_.buttons & 1024)) { //X button or Start button + // reset(); + // return; + //} std::string vendorid = joystick_state_.pid_vid.substr(0, joystick_state_.pid_vid.find('&')); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h index fb10636acc..faa6428d1a 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.h @@ -81,8 +81,6 @@ class ACarPawn : public AWheeledVehicle void setupInputBindings(); - void reset(bool disable_api_control = true); - // Begin Actor interface virtual void Tick(float Delta) override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index 9c826621c7..e104d6caf9 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -1,7 +1,6 @@ #include "CarPawnApi.h" #include "AirBlueprintLib.h" - CarPawnApi::CarPawnApi(VehiclePawnWrapper* pawn, UWheeledVehicleMovementComponent* movement) : pawn_(pawn), movement_(movement) { @@ -43,6 +42,13 @@ msr::airlib::CollisionInfo CarPawnApi::getCollisionInfo() const return pawn_->getCollisionInfo(); } +bool CarPawnApi::armDisarm(bool arm) +{ + //TODO: implement arming for car + unused(arm); + return true; +} + std::vector CarPawnApi::simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const { std::vector request = { ImageCaptureBase::ImageRequest(camera_id, image_type) }; @@ -118,6 +124,7 @@ void CarPawnApi::reset() auto phys_comps = UAirBlueprintLib::getPhysicsComponents(pawn_->getPawn()); UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { pawn_->reset(); + for (auto* phys_comp : phys_comps) { phys_comp->SetPhysicsAngularVelocity(FVector::ZeroVector); phys_comp->SetPhysicsLinearVelocity(FVector::ZeroVector); diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h index db5a40639d..54f08d942f 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h @@ -42,6 +42,7 @@ class CarPawnApi : public msr::airlib::CarApiBase { virtual void enableApiControl(bool is_enabled) override; virtual bool isApiControlEnabled() const override; + virtual bool armDisarm(bool arm) override; virtual const CarApiBase::CarControls& getCarControls() const override; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index c25528c349..1f9e06087a 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -2,6 +2,7 @@ #include "ConstructorHelpers.h" #include "AirBlueprintLib.h" #include "common/AirSimSettings.hpp" +#include "AirBlueprintLib.h" #include "Car/CarPawn.h" #ifndef AIRLIB_NO_RPC @@ -201,18 +202,11 @@ void ASimModeCar::createVehicles(std::vector& vehicles) void ASimModeCar::reset() { - //find all vehicle pawns - { - TArray pawns; - UAirBlueprintLib::FindAllActor(this, pawns); - - //set up vehicle pawns - for (AActor* pawn : pawns) - { - //initialize each vehicle pawn we found - TVehiclePawn* vehicle_pawn = static_cast(pawn); - vehicle_pawn->reset(); - } + msr::airlib::VehicleApiBase* api = getVehicleApi(); + if (api) { + UAirBlueprintLib::RunCommandOnGameThread([api]() { + api->reset(); + }, true); } Super::reset(); diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp index 59b58b7b7f..4b644c0714 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/MultiRotorConnector.cpp @@ -319,23 +319,14 @@ msr::airlib::MultirotorApi* MultiRotorConnector::getApi() const //*** Start: UpdatableState implementation ***// void MultiRotorConnector::reset() { - if (UAirBlueprintLib::IsInGameThread()) - resetPrivate(); - else { - //schedule the task which we will execute in tick event when World object is locked - reset_task_ = std::packaged_task([this]() { resetPrivate(); }); - std::future reset_result = reset_task_.get_future(); - reset_pending_ = true; - did_reset_ = false; - reset_result.wait(); - } + resetPrivate(); } void MultiRotorConnector::resetPrivate() { VehicleConnectorBase::reset(); - //TODO: should this be done in MultiRotor.hpp + //TODO: should this be done in MultiRotor.hpp? //controller_->reset(); rc_data_ = RCData(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index cde38fa5e7..349753c194 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -71,7 +71,6 @@ msr::airlib::SimModeApiBase* ASimModeBase::getSimModeApi() const return simmode_api_.get(); } - void ASimModeBase::setupTimeOfDay() { sky_sphere_ = nullptr; @@ -308,6 +307,7 @@ void ASimModeBase::stopRecording() FRecordingThread::stopRecording(); } + //************************* SimModeApi *****************************/ ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) @@ -315,6 +315,11 @@ ASimModeBase::SimModeApi::SimModeApi(ASimModeBase* simmode) { } +void ASimModeBase::SimModeApi::reset() +{ + simmode_->reset(); +} + msr::airlib::VehicleApiBase* ASimModeBase::SimModeApi::getVehicleApi() { return simmode_->getVehicleApi(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 123ddca578..839a06ae80 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -82,6 +82,7 @@ class AIRSIM_API ASimModeBase : public AActor public: SimModeApi(ASimModeBase* simmode); virtual msr::airlib::VehicleApiBase* getVehicleApi() override; + virtual void reset() override; virtual bool isPaused() const override; virtual void pause(bool is_paused) override; virtual void continueForTime(double seconds) override; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 1115cb963f..7929fcfa71 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -1,5 +1,6 @@ #include "SimModeWorldBase.h" #include +#include "AirBlueprintLib.h" void ASimModeWorldBase::BeginPlay() @@ -131,7 +132,9 @@ void ASimModeWorldBase::Tick(float DeltaSeconds) void ASimModeWorldBase::reset() { - physics_world_->reset(); + UAirBlueprintLib::RunCommandOnGameThread([this]() { + physics_world_->reset(); + }, true); Super::reset(); } diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp index 51dc873a5b..e3d72569e8 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.cpp @@ -217,12 +217,15 @@ int VehiclePawnWrapper::getCameraCount() void VehiclePawnWrapper::reset() { state_ = initial_state_; + pawn_->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); +} + +//void playBack() +//{ //if (pawn_->GetRootPrimitiveComponent()->IsAnySimulatingPhysics()) { // pawn_->GetRootPrimitiveComponent()->SetSimulatePhysics(false); // pawn_->GetRootPrimitiveComponent()->SetSimulatePhysics(true); //} - pawn_->SetActorLocationAndRotation(state_.start_location, state_.start_rotation, false, nullptr, ETeleportType::TeleportPhysics); - //TODO: refactor below code used for playback //std::ifstream sim_log("C:\\temp\\mavlogs\\circle\\sim_cmd_006_orbit 5 1.txt.pos.txt"); //plot(sim_log, FColor::Purple, Vector3r(0, 0, -3)); @@ -233,7 +236,7 @@ void VehiclePawnWrapper::reset() //plot(sim_log, FColor::Purple, Vector3r(0, 0, -3)); //std::ifstream real_log("C:\\temp\\mavlogs\\square\\real_cmd_012_square 5 1.txt.pos.txt"); //plot(real_log, FColor::Yellow, Vector3r(0, 0, -3)); -} +//} const VehiclePawnWrapper::GeoPoint& VehiclePawnWrapper::getHomePoint() const { diff --git a/docs/apis.md b/docs/apis.md index d7e0c3ce5c..aace2a9184 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -85,7 +85,7 @@ You can find a ready to run project in HelloCar folder in the repository. ## Common APIs -* `reset`: This resets the vehicle to its original starting state. +* `reset`: This resets the vehicle to its original starting state. Note that you must call `enableApiControl` and `armDisarm` again after the call to `reset`. * `confirmConnection`: Checks state of connection every 1 sec and reports it in Console so user can see the progress for connection. * `enableApiControl`: For safety reasons, by default API control for autonomous vehicle is not enabled and human operator has full control (usually via RC or joystick in simulator). The client must make this call to request control via API. It is likely that human operator of vehicle might have disallowed API control which would mean that enableApiControl has no effect. This can be checked by `isApiControlEnabled`. * `isApiControlEnabled`: Returns true if API control is established. If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, the `isApiControlEnabled` should return true. diff --git a/docs/reinforcement_learning.md b/docs/reinforcement_learning.md index 6a3c4f07d8..9ee1bd4977 100644 --- a/docs/reinforcement_learning.md +++ b/docs/reinforcement_learning.md @@ -89,6 +89,8 @@ If the episode terminates then we reset the vehicle to the original state via: ``` client.reset() +client.enableApiControl(True) +client.armDisarm(True) car_control = interpret_action(1) // Reset position and drive straight for one second client.setCarControls(car_control) time.sleep(1) From 86d2e770e9bdca17bdbc8ff5f1ae4a7a1b1449b2 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 00:12:27 -0700 Subject: [PATCH 107/121] potential fix for https://github.com/Microsoft/AirSim/issues/1027 --- .../AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 28d939593f..1309a65439 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -94,8 +94,10 @@ struct DirectInputJoyStick::impl{ g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; - g_pWheelRumbleHandle->SetParameters(&g_sWheelRumbleConfig, DIEP_DIRECTION | - DIEP_TYPESPECIFICPARAMS | DIEP_START); + if (g_pWheelRumbleHandle) { + g_pWheelRumbleHandle->SetParameters(&g_sWheelRumbleConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } } const JoystickState& getState(bool update_state = true) From f2841650753549d518c5a458e20af1939989854a Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Fri, 27 Apr 2018 02:58:33 -0700 Subject: [PATCH 108/121] fix broken build. --- .../include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index ed07d69a61..6e6a92c67e 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -18,7 +18,6 @@ class MultirotorRpcLibClient : public RpcLibClientBase { public: MultirotorRpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451, uint timeout_ms = 60000); - bool armDisarm(bool arm); void setSimulationMode(bool is_set); bool takeoff(float max_wait_ms = 15); bool land(float max_wait_seconds = 60); From 126900e52a54bd6ee1cc36476dc23fb22c23051b Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 13:58:29 -0700 Subject: [PATCH 109/121] Isolate focefb to thrustmaster, fix for https://github.com/Microsoft/AirSim/issues/1024 --- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index 209ae7fcb7..bb4cb72f11 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -344,8 +344,6 @@ void ACarPawn::Tick(float Delta) updateCarControls(); - updateForceFeedback(); - updateKinematics(Delta); // update phsyics material @@ -382,6 +380,8 @@ void ACarPawn::updateCarControls() joystick_controls_.steering = joystick_state_.left_x; joystick_controls_.throttle = (-joystick_state_.right_z + 1) / 2; joystick_controls_.brake = (joystick_state_.left_y + 1) / 2; + + updateForceFeedback(); } // Anything else, typically Logitech G920 wheel else { From 53ad59421c81d3680894870f7f415629b1856e3a Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 14:42:19 -0700 Subject: [PATCH 110/121] solutions and projects upgraded to VS2017 --- AirLib/AirLib.vcxproj | 8 ++++---- AirLibUnitTests/AirLibUnitTests.vcxproj | 8 ++++---- DroneServer/DroneServer.vcxproj | 8 ++++---- DroneShell/DroneShell.vcxproj | 8 ++++---- Examples/Examples.vcxproj | 8 ++++---- HelloCar/HelloCar.vcxproj | 8 ++++---- HelloDrone/HelloDrone.vcxproj | 8 ++++---- MavLinkCom/MavLinkCom.vcxproj | 6 +++--- MavLinkCom/MavLinkMoCap/MavLinkMoCap.sln | 14 +++++++++++--- MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj | 10 +++++----- MavLinkCom/MavLinkTest/MavLinkTest.sln | 16 +++++++++++++--- MavLinkCom/MavLinkTest/MavLinkTest.vcxproj | 2 +- 12 files changed, 61 insertions(+), 43 deletions(-) diff --git a/AirLib/AirLib.vcxproj b/AirLib/AirLib.vcxproj index 8e9b6d42e2..75b853bfb5 100644 --- a/AirLib/AirLib.vcxproj +++ b/AirLib/AirLib.vcxproj @@ -210,26 +210,26 @@ StaticLibrary true - v140 + v141 Unicode StaticLibrary false - v140 + v141 true Unicode StaticLibrary true - v140 + v141 Unicode StaticLibrary false - v140 + v141 true Unicode diff --git a/AirLibUnitTests/AirLibUnitTests.vcxproj b/AirLibUnitTests/AirLibUnitTests.vcxproj index 6c31f9984e..8987b3e465 100644 --- a/AirLibUnitTests/AirLibUnitTests.vcxproj +++ b/AirLibUnitTests/AirLibUnitTests.vcxproj @@ -31,26 +31,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/DroneServer/DroneServer.vcxproj b/DroneServer/DroneServer.vcxproj index a4ff780221..218679025f 100644 --- a/DroneServer/DroneServer.vcxproj +++ b/DroneServer/DroneServer.vcxproj @@ -39,26 +39,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/DroneShell/DroneShell.vcxproj b/DroneShell/DroneShell.vcxproj index a043be3168..a35d18105c 100644 --- a/DroneShell/DroneShell.vcxproj +++ b/DroneShell/DroneShell.vcxproj @@ -31,26 +31,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/Examples/Examples.vcxproj b/Examples/Examples.vcxproj index a9774e7c93..40bac1c0f0 100644 --- a/Examples/Examples.vcxproj +++ b/Examples/Examples.vcxproj @@ -39,26 +39,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/HelloCar/HelloCar.vcxproj b/HelloCar/HelloCar.vcxproj index 4b6dc3f573..84a6ce3db4 100644 --- a/HelloCar/HelloCar.vcxproj +++ b/HelloCar/HelloCar.vcxproj @@ -42,26 +42,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/HelloDrone/HelloDrone.vcxproj b/HelloDrone/HelloDrone.vcxproj index 69bfd60612..c2d0ed7652 100644 --- a/HelloDrone/HelloDrone.vcxproj +++ b/HelloDrone/HelloDrone.vcxproj @@ -42,26 +42,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj index 8691f8c1f4..d566480c09 100644 --- a/MavLinkCom/MavLinkCom.vcxproj +++ b/MavLinkCom/MavLinkCom.vcxproj @@ -2,7 +2,7 @@ true - v140 + v141 @@ -47,7 +47,7 @@ StaticLibrary true Unicode - 10.0.15063.0 + 8.1 StaticLibrary @@ -60,7 +60,7 @@ false true Unicode - 10.0.15063.0 + 8.1 StaticLibrary diff --git a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.sln b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.sln index d696646911..1e1f4336f2 100644 --- a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.sln +++ b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.sln @@ -1,32 +1,40 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 14 -VisualStudioVersion = 14.0.25420.1 +# Visual Studio 15 +VisualStudioVersion = 15.0.26430.15 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkMoCap", "MavlinkMoCap.vcxproj", "{9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}" EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkCom", "..\MavLinkCom\MavLinkCom.vcxproj", "{8510C7A4-BF63-41D2-94F6-D8731D137A5A}" +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkCom", "..\MavLinkCom.vcxproj", "{8510C7A4-BF63-41D2-94F6-D8731D137A5A}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|ARM = Debug|ARM Debug|x64 = Debug|x64 Debug|x86 = Debug|x86 + Release|ARM = Release|ARM Release|x64 = Release|x64 Release|x86 = Release|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Debug|ARM.ActiveCfg = Debug|Win32 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Debug|x64.ActiveCfg = Debug|x64 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Debug|x64.Build.0 = Debug|x64 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Debug|x86.ActiveCfg = Debug|Win32 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Debug|x86.Build.0 = Debug|Win32 + {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Release|ARM.ActiveCfg = Release|Win32 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Release|x64.ActiveCfg = Release|x64 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Release|x64.Build.0 = Release|x64 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Release|x86.ActiveCfg = Release|Win32 {9E9D74CE-235C-4A96-BFED-A3E9AC8D9039}.Release|x86.Build.0 = Release|Win32 + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|ARM.ActiveCfg = Debug|ARM + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|ARM.Build.0 = Debug|ARM {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x64.ActiveCfg = Debug|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x64.Build.0 = Debug|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x86.ActiveCfg = Debug|Win32 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x86.Build.0 = Debug|Win32 + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|ARM.ActiveCfg = Release|ARM + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|ARM.Build.0 = Release|ARM {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x64.ActiveCfg = Release|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x64.Build.0 = Release|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x86.ActiveCfg = Release|Win32 diff --git a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj index e8c7a148cd..fb6b980a5b 100644 --- a/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj +++ b/MavLinkCom/MavLinkMoCap/MavLinkMoCap.vcxproj @@ -1,5 +1,5 @@  - + Debug @@ -29,26 +29,26 @@ Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode Application true - v140 + v141 Unicode Application false - v140 + v141 true Unicode diff --git a/MavLinkCom/MavLinkTest/MavLinkTest.sln b/MavLinkCom/MavLinkTest/MavLinkTest.sln index 946fe9eb9a..7b83d53c54 100644 --- a/MavLinkCom/MavLinkTest/MavLinkTest.sln +++ b/MavLinkCom/MavLinkTest/MavLinkTest.sln @@ -1,32 +1,42 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 14 -VisualStudioVersion = 14.0.25420.1 +# Visual Studio 15 +VisualStudioVersion = 15.0.26430.15 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkTest", "MavLinkTest.vcxproj", "{25EB67BE-468A-4AA5-910F-07EFD58C5516}" EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkCom", "..\MavLinkCom\MavLinkCom.vcxproj", "{8510C7A4-BF63-41D2-94F6-D8731D137A5A}" +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkCom", "..\MavLinkCom.vcxproj", "{8510C7A4-BF63-41D2-94F6-D8731D137A5A}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|ARM = Debug|ARM Debug|x64 = Debug|x64 Debug|x86 = Debug|x86 + Release|ARM = Release|ARM Release|x64 = Release|x64 Release|x86 = Release|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution + {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|ARM.ActiveCfg = Debug|ARM + {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|ARM.Build.0 = Debug|ARM {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|x64.ActiveCfg = Debug|x64 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|x64.Build.0 = Debug|x64 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|x86.ActiveCfg = Debug|Win32 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Debug|x86.Build.0 = Debug|Win32 + {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|ARM.ActiveCfg = Release|ARM + {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|ARM.Build.0 = Release|ARM {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|x64.ActiveCfg = Release|x64 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|x64.Build.0 = Release|x64 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|x86.ActiveCfg = Release|Win32 {25EB67BE-468A-4AA5-910F-07EFD58C5516}.Release|x86.Build.0 = Release|Win32 + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|ARM.ActiveCfg = Debug|ARM + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|ARM.Build.0 = Debug|ARM {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x64.ActiveCfg = Debug|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x64.Build.0 = Debug|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x86.ActiveCfg = Debug|Win32 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Debug|x86.Build.0 = Debug|Win32 + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|ARM.ActiveCfg = Release|ARM + {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|ARM.Build.0 = Release|ARM {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x64.ActiveCfg = Release|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x64.Build.0 = Release|x64 {8510C7A4-BF63-41D2-94F6-D8731D137A5A}.Release|x86.ActiveCfg = Release|Win32 diff --git a/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj b/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj index 202297d007..0267440a29 100644 --- a/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj +++ b/MavLinkCom/MavLinkTest/MavLinkTest.vcxproj @@ -27,7 +27,7 @@ - v140 + v141 {25EB67BE-468A-4AA5-910F-07EFD58C5516} From aaa7d80c32323a3ecbbb9f8dc4637542d58d4192 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 18:08:48 -0700 Subject: [PATCH 111/121] Update compress code for UE4.18, issue: https://github.com/Microsoft/AirSim/issues/433 --- Unreal/Environments/Blocks/Blocks.uproject | 2 +- .../Blocks/Config/DefaultEngine.ini | 2 + .../Blocks/Config/DefaultGame.ini | 36 ++++++++- .../Blocks/Content/AirSimAssets.umap | Bin 0 -> 41116 bytes .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 74 +++++++++++++++--- .../Plugins/AirSim/Source/AirBlueprintLib.h | 18 +++-- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 2 + .../Plugins/AirSim/Source/RenderRequest.cpp | 3 +- build.cmd | 4 +- build.sh | 8 +- setup.sh | 30 +++---- 11 files changed, 139 insertions(+), 40 deletions(-) create mode 100644 Unreal/Environments/Blocks/Content/AirSimAssets.umap diff --git a/Unreal/Environments/Blocks/Blocks.uproject b/Unreal/Environments/Blocks/Blocks.uproject index 47b5e6be77..7ed3332dc4 100644 --- a/Unreal/Environments/Blocks/Blocks.uproject +++ b/Unreal/Environments/Blocks/Blocks.uproject @@ -1,6 +1,6 @@ { "FileVersion": 3, - "EngineAssociation": "4.16", + "EngineAssociation": "4.18", "Category": "", "Description": "", "Modules": [ diff --git a/Unreal/Environments/Blocks/Config/DefaultEngine.ini b/Unreal/Environments/Blocks/Config/DefaultEngine.ini index ef8afbbc34..14ff4fd8d6 100644 --- a/Unreal/Environments/Blocks/Config/DefaultEngine.ini +++ b/Unreal/Environments/Blocks/Config/DefaultEngine.ini @@ -57,6 +57,7 @@ bSuppressFaceRemapTable=False bSupportUVFromHitResults=False bDisableActiveActors=False bDisableCCD=False +bEnableEnhancedDeterminism=False MaxPhysicsDeltaTime=0.033333 bSubstepping=False bSubsteppingAsync=False @@ -65,5 +66,6 @@ MaxSubsteps=6 SyncSceneSmoothingFactor=0.000000 AsyncSceneSmoothingFactor=0.990000 InitialAverageFrameRate=0.016667 +PhysXTreeRebuildRate=10 diff --git a/Unreal/Environments/Blocks/Config/DefaultGame.ini b/Unreal/Environments/Blocks/Config/DefaultGame.ini index 50599ff00b..78da513505 100644 --- a/Unreal/Environments/Blocks/Config/DefaultGame.ini +++ b/Unreal/Environments/Blocks/Config/DefaultGame.ini @@ -9,5 +9,39 @@ ProjectDisplayedTitle=NSLOCTEXT("[/Script/EngineSettings]", "8F8B6B2A472F9FDFB69 ProjectDebugTitleInfo=NSLOCTEXT("[/Script/EngineSettings]", "F31D7C524A9E9BC66DD2AA922D309408", "Blocks Environment for AirSim") [/Script/UnrealEd.ProjectPackagingSettings] -IncludeDebugFiles=True +Build=IfProjectHasCode +BuildConfiguration=PPBC_Development +StagingDirectory=(Path="C:/temp") FullRebuild=True +ForDistribution=False +IncludeDebugFiles=False +BlueprintNativizationMethod=Disabled +bIncludeNativizedAssetsInProjectGeneration=False +UsePakFile=True +bGenerateChunks=False +bGenerateNoChunks=False +bChunkHardReferencesOnly=False +bBuildHttpChunkInstallData=False +HttpChunkInstallDataDirectory=(Path="") +HttpChunkInstallDataVersion= +IncludePrerequisites=True +IncludeAppLocalPrerequisites=False +bShareMaterialShaderCode=True +bSharedMaterialNativeLibraries=True +ApplocalPrerequisitesDirectory=(Path="") +IncludeCrashReporter=False +InternationalizationPreset=English +-CulturesToStage=en ++CulturesToStage=en +bCookAll=False +bCookMapsOnly=False +bCompressed=True +bEncryptIniFiles=False +bEncryptPakIndex=False +bSkipEditorContent=False ++MapsToCook=(FilePath="/Game/FlyingCPP/Maps/FlyingExampleMap") ++MapsToCook=(FilePath="/Game/AirSimAssets") +bNativizeBlueprintAssets=False +bNativizeOnlySelectedBlueprints=False + + diff --git a/Unreal/Environments/Blocks/Content/AirSimAssets.umap b/Unreal/Environments/Blocks/Content/AirSimAssets.umap new file mode 100644 index 0000000000000000000000000000000000000000..289c85db30a26a7ba10450c75dc3d277713a97b0 GIT binary patch literal 41116 zcmeHQ33ybs_D=x?R0L5#6l4_vfzq<02<^0mB1>&c1zgH>=F$#KXUZ%<6_F>PD2V!Z zMNx~p?>&_#u1`@GK~SCxii!%NB0d*f5JUv^e}1{iG;=qm*zwcvHQ$#eIlm+)Ip^dg zxp!u|dT#f-4*d4pZ#PU5tsFe?B%%$C9D03D)^gWJC*5#kxP9AJ zrx9&;+47xVWZty**!R=!$#^cO7wWx`#~65x2?jJu>apGQ_uLfvL!refjJYXY)`78X zQEr?Iga$WOkGm-izq;Y*k&m`L_wTLt{@C@jg}c!l?a)^P)|}O6RhJBACsOImGsk83 zJu_ePYwobtvv04ofxH4^FE1HrSAm-Gu;SP6`XkefhJ0HUC&m7d^nSxsXrL@L!xt*> zm8WLCPKfDBJ07#ig+(fsVeR-s~8O-88}h>XyDrDYMe?(Tb5#Ax|$uP+*A19pce zHbFfuIWhA6x6byp2C=-vN6p82JpO3d7Yw+9fpEkfh_Hhvy>n9=iwcF>%t$n(75FaE z*tta|hmJ>`P{>`G7YbHrp-3gmUH++$^NU2>p0Yf5i27%}etT#qq>&Nz`hu=tc?Fpy zk;l2$Mljz@w`Z!02wF&}CueJD8O+)U6c5iHQ(TmhpPP|6Dx1x?byu(j)haPgW3;=1 zHTkS?4J2KX>7J?;(a^M+S?-9tFgRKZm$6s6zV&~!0$7R5?e|3e?ubUp3z6bRjm;Vz z^pboBPxNL%0wox3G@^M&LB!N^z7`JpqY;u8D?ee;s#BFZ7FBNfZtVexqa?fB7Y_TT zYZzN@-lOR-oRZ;MfUGg1$%f;K*ra#bFX7|P4QG3Or0%?sHr=Pq;Hqbyytj(b9Zi2s zR`J*h_q3?SW=?+ZpKunLel6g|=-JQv>)nHa#ny?742k9t6MdNV%DSx4$!Zlj7O%_i z4u@H{J1TlMBfMm^7AmDtlX84+f0(_}^!WMBV|3WB&l84Z+5Q`wKFOy#T8p^-exCN3N z+pNLzYp`faNSYpBq!L5fck^ux_^4yF@Kn%;?BBW<&LA@wi3H2zO!n+;v+r+j3B`|L z?Bc2AST*r}>zFOuF9gFlYO#y0JbyxKEF7Z8oEMSrnR-(WS3;g|dN7g~Bu|(TnW%+= z?6J$gyo#GvzUGmpt&2F!MtV|KfptlKFj`s`AXQ*an;#px?P#zFMQLI5+Sk&{8KM+3 z7xv7A(s6uzapc~tY2chayi4-R6@K4jT?_Y(TKYD(gn|kUt1ARPokrigj<42nVQrK< z;0=4+6`Ctbh7u@c^(&U22*H*RD11z&FAxBO2h1O45(95ox5gLaTnx##4NBxoFVuKMZxcKDz(X1Nh8od3-z@u0KF>>{0 zMwYYXzF)6}76dBRZxMO4a*dslGB=8CHcP9Bl;vrmFsUy>KFj5)Vo&# z72{3+#*b3JX&Y+~M_uF#o4V}3`p{Kf94f?j z_wC{*r@$1mw6HIvc{792fLH9L%hsm9sB_{_j*lBJ3Ccs8m%CJBSt)n*;B9@ACr4wJ z%y-M;5r2QOGrIMKE8HRSGy1Bejnr!z@cxeq7pEmwH-79xHr{b3%rLupD|q+&dB0%= zWV>9&=LbW6Zvm{>!>+jRnOopOvRy%%1>gM3pb)jeUW3^#VGW@{w@#q02uw722>w z2M>;&3aBAl*P`8PbHA05|19=a>I{oJOY@3etG90fa7L9rcDJ-%avdu=-%C$!uET`Pub?hvc| zrrmm-;)zjS)UbAQ14N;XlXjKnU7BsuNz_b|)|`OFnH!i)BeC!uQ@0L&7|zbFLcCA{ zw;osn+s?(|JK*-~63wI=TpFA2Dq3ibrLzmQXF6MBeb`qsx4EpbzU-T>-b<~qer!U& zlr}ib(OCG2hRu%V9s?zTAg>qqztw*v4(|3oVdtTX4`Oc}rA;SiHl@c6H^FFlDKkpD zGyF}imo4crZ9Ht518_JM!BB*R7Gwn_&klnf$V0hu;6LTRq9nHs=3&Q4w0@VK#S9s{@#eL}Ngwk)Pa+`Hyzbq-B8PTao4u;%Lk| z)Z6_O405zqUZREgO8PN>TElA67`i00MeSBR1%Ia35?8?4UtD%JJPP;FvKkrq&fBn)@?F%*b(+wNmga$|Z0%M|>@z}}S5r+$d+?3d$ zHgi9Rt?5_B&4X{7%qJ6^u9ee?03M#*-tQ|e3`izs#aHZUG!h~n+9u@Ss{k}`M^5mP? zg1iiYJ+q(qv(tAY6FAJQcn8Eat0wPA8|0muJJwl9@bIpZXL7qrVZT-aCnFzsxc3}?7E@6Su_yw zm23RGr3<&)>%}?n-UW2IFyFw+%Zsjt53oejv)KB<*RR6y(Grihf?ZRKbFt3#sxbo| zak94<>zq#Rnik-9n5#;5ti(zy&`ROoxLLT2cf&o){x!M#VJgKBVE+6SVLnqlL(ueH zBVU`W(Tb#%d)@vIzUAmtI>D2V#zs)R%vKJ_Z^DO!Q?lIwdBcHdbL`G|9O0Yn^Waq9 ze%B|(Fmuqb$HmkYI)0J{rzHVZTgRW?WERKx9V&h#f$y_BVlfm%GnCoPJZb=0o- z5*i=pDxi&(6sCEFkg!2coiVF#Lqdz_I!Ej7fi?}i7`%9hu3xbDmS5bXZ9Pm?AkY@; z{@t7>;npQhxF)vo-UCu}KpEiq&LW-X6a z4X(cBWw=~UjTgr5Ug=Y@kt1Lr2rtkZ4!l!AkQnYW2-^wkrDsjpHo{VEun!6QKwv2% zm37fB+t99$D8Elwjsfc{%0Cm<&w!mR$_ELPc{ztnf&EGtRD@uK_0SC6 z)QQ4ZJ`2duyWo9TcpbHHA8 zz+N|C($3-QWM0Zzmf_1QZQ235!~wh10h{H3U1q@KnghSj4VYYWunFw!2yzZ8_Aec> z#8x<9PdZ>v+hDMls{|(3jNH4hj}14plfD7-i#T9Wfyp%^*FW0bA+YZ$FS6_c%D6?L z5v{OpB9BiWBod@Ape|#5%65>q%;RjzXd}pu&oLx{K_O?;I?YGrgFV)Y&&XQl1OIEqCtS;X;0LYvEUjfe4Q=?Ws|7y0 z6+3|2#1&qKt3~FUB=9*?GNDxRnNh3|9kNdLW}Hu%(vRfBCMiURY)$w~QTma5u1*3U zzXhM>ZiVQOb(+uDiXA`=kBENwekvN?Q44(bD15-;AA%2TFGJ)Hm52-_U@h2J6Ad}) z(&R_KQfN3Iz+WR^K8tydu^}FjIZPt?C&CB!NYq#PyjlxJ(~ zJI%+64|Ie1NIvwwH=*Ff=fBE)(Em!M$FU+`nFKyBTJYJB1U{=R__Wd#qC>VOa$BqP zBjq-^7WjOw=%+sM$W{1AAM|7r_?&COr^950=#X`in{hs)m44)Wu1x};d<#AYlE7z% z1s|D0PYa*I+##Fj ze89(Q{roKne5}^b?j-QBT0axi!&WWH&1(IuO9G!NWxUef(xxgzhipykV58EHhZDttd5_PnFVvRLc!X;A6FZen|o!tM%gxDwA}`CMq{Pr?6T-Tav)XYW-wX zI7~u;YBC?I_47y)_*ku<*3*!VZ&AphV{bK33~zSrYhIt)C|H<&QFUt87i?W3_%NlfcJn{d|@L zK2?f9Jz8F(5t87i=X0?7+CV`LD`sp^^YV>BB(|mRro*!(AHLrfZaC0s5!TraI z&kME82j@{MKA+bzAMD#!e444o9+Rs%lc;{+cdYmfs%1WS4JY5_tM9W)lE6oPE*O9G$EEchHPKMW!;yTr-fK2!Vw?BH{SkK9{RYk|*Bg%6%9 zcTol#27$uNB=A|Tcg_FaD+KYCG(rWl$~wu-IG@Lr4kVw&(l`{%Dq9miPbeKoK54TR zqC>VOe4bMJk$m31Tp>DS6Xk>F*H3}*A!77d;}Y|sEP^+1SdYQ;XI0%D?WeMl=96eBE@Go+E6@&Vq1!* zQY@f|yI3K`aTJ?VJdxsg6!BR)jIATZ(_D*{#ZDC4Q#_qwXNt#A%%O@`vrI{83i=zi${%HwKgue9lvVz)b(KHLDu0w!{wS;bQC9h*tnx=$<&Uz;AHMBXvIn_V;qTP) z35GKKri#Ots%7}vSExVuTG%tn8x7?RhVsjX@_JFm8d|61U!pwLKl;V`U27<>F_c#u z%Bz$z*4>L#mg^37sg_|UY8iS{%aEfi!*8EUaTG;7FC0U0EX9i`UPuxC0&>8|198rh za#=}vRZh=~GUol9l6O-Xd$CAQV;T#pn1Jdw@OIENzEA89Rq|hJ zo?7j;fQ zXQThH8~p8{KIwz_=1h5ZFaA)#D0qMr?-qF^*g+9Y6`K(eV@5E6u;DTeS%7G@+785N zVWl02W5ilJ5a){*?LZtn9=8Jv2v}|h<`eLQ9f&i`Q+6N@H&g6Dcmcm12(@>y1J5Ji zIVZ4@0N)cSSn+@pM%>Sk@pBoVt{kualfqAHD5DJmPBjQp)f~W3FjjEDY#|QF3j&3f zVFs`hNFVBf{-9vhz~qx7*>TJgJ5ojX1OyX^!{L=2vH*(+z%UR@Ejthqt^~mZwj*K{ z5DF%+6A|Hj5KLfuBEANMf(bmGh#NU%0d^+9DBYM4jv;W9UCSH-Y%KxcKkZsVX+|@N z`9Ek({Xw!m{d=3tK^lylDnF@WCYWk_iVzm032{RoP#83tkm|crMO)QlVc4o3hksku zcO%eN^=A-RMG-;HDy9Z_L0i>B7PhMINuaIjdk|==`eO;SRXsLSTh#-!RXx^f4OE|& znCdYTOck<#upmu{8~T94pwWa>pH3BRRo{z1Th-$ZXRG=&0&P{FN?;X51e5Cf5Yblk zy$Q5cJuZ;8s_#pnt?G{>&{p*$2((o_KwH(rCD%ap>4~WxGr?3L3kVC+gt(y(C=41+ zNcFf_+p4}lfwrp0RoqtfxTD*uegJ`06cJ3SKZ}UAsvk(8t?GvmXsddt-B$HY2((o_ zq+_dkfVQf~PFw@k_eo6km<6T^SwL8jCd3VWKw;2mLaNWCingl9gA-fTPbAP*^%(@( zs(vVeRTL3Ss&^65R`tUOv{ijJfwrp8BG6X##}jC)`jG_Msve-N>am^IK=pkSQ$1#a zsScwEVL_S@H}nC8L8GebaW+8cswc6(DY5Uvm~_yRD#5uRn83zF+{_`yJR+5|^olam zM8;Nd=tZ`APgFiewmzCN`cLPo6M>6EFpUVqRV!l2A($%W5HX>O*vAv9I3m%CSYrwG z2#ZoHLbV8{$v{Z9Gd+($9QqKKX zQ$3c-ph*WqS;q(8u;3{FzW}JokInkCAN{`|xbpH+=~+%VRWD}Gbo1ZTqDMzExapFW zLIq4SsX?737=h{E1;-AQW@h9J?AxbbX5aL*e(4zl2Moy0$R5y-Nc~;?GtyiG;>0A{ z*(Apax=e>D^-dOf7rw)Nr%*M2hkLXeHq5mhY` zISQE$Q|hRrL0*nRrg9``B1cpI@iTVp{;BE6N6t9VCVKsA*8($+##pXphx$L&o$UsJ zJ-L=8QO66`EJ6x@#fgGPP^4FM-0kaRD5QntuPj3xW&f!4RmTg~^840Toeck4G92*- zuCF>?zuz|MWcb&TAs(FkQTtaNuitMQbu#>G$q>&Q|ETp<$LsgoMx6}*S~5J5hO0hE zvb|V0PqZRlCMVP*SS3^uPhu0QnD-Z~_$-YXK|XetPY^%T$rgX-^vI$YlrF{=tx=_N zx~M8U8Y9Y*ZtCeiLBR`J1bHzm(u?{hw~*&gA1N&rfP8RCpX{HMEr)Of^XN$P*szwd z_N!~b!|xP>!^=_xmw2oy#lb&DE-Ojm@uJcjdK>fK>jmjvk;bO}1${{LIze12QYk?G zStrmI3qZ1iAtTtnQqbY*zf!oi!@zf#ZHFOMQftGI!;u4&rX}Ym%QBVqVrfE_Wb1-z z>8+tO0%Z#DYoImuw{@$iZWVEdZ@Y?W z>Aj%bX6p7LvA@P8zwfwEORp;GR75&S(uuB?o_5!%D0wQXr3a>UDoUP;YU!EV|CEX< z#YSVg*yCg2m)L=$2)NV^98JJ1J8%pEm)U`137BpNUPM6H4#daQO6)*9+_=#W%p>5x zcHqSX*nTz|E#;$*_e2l!oWy@fq55A}I(&~%Jz~q&CS`!t5mlTd#wD-sQlq;B-IRP{ zXpkYw8%#9?VS=H7l*qsB8pwGV({~LWC7<(M29BAce5c*ej}d_B0pOmmJ+8)f;I5VJ zUB=0x?#H5mN!xA$M%aO%PZXd$E&p^tj}pC-DIyT&r%G2}Yr&--O1N%V_KA<@{Bd@3j= z-~H(DW#{x4U{_PKX9Pv^xkc5)Qj00mL@HeR3$1>KZvH#Cr^ zmf5D7l7Jp5ek97qmxYsMy&WPG6sburjLjL=Nq|PVr0|~u0BA%QyVbr!{X4|g&DJ|K z8#+LMW^QjgHIv`uFgNRF+u{B89nKlr-L}~SPR;Pr9>F{gxfxnXe<IBW+lGqzxZB(uNNnX~QN*+HiqrXk5g&Me=ZsNZW|iSg}yh z`46`sFMVzc0T?YZh20 CamelCase parameters -> camel_case */ -bool UAirBlueprintLib::log_messages_hidden = false; -uint32_t UAirBlueprintLib::FlushOnDrawCount = 0; -msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method = +bool UAirBlueprintLib::log_messages_hidden_ = false; +uint32_t UAirBlueprintLib::flush_on_draw_count_ = 0; +msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method_ = msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName; +IImageWrapperModule* UAirBlueprintLib::image_wrapper_module_ = nullptr; void UAirBlueprintLib::LogMessageString(const std::string &prefix, const std::string &suffix, LogDebugLevel level, float persist_sec) { @@ -122,31 +125,38 @@ void UAirBlueprintLib::enableViewportRendering(AActor* context, bool enable) // Do this only if the main viewport is not being rendered anyway in case there are // any adverse performance effects during main rendering. //HACK: FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves - if (FlushOnDrawCount == 0) + if (flush_on_draw_count_ == 0) viewport->GetGameViewport()->IncrementFlushOnDraw(); } else { viewport->EngineShowFlags.SetRendering(true); //HACK: FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves - if (FlushOnDrawCount > 0) + if (flush_on_draw_count_ > 0) viewport->GetGameViewport()->DecrementFlushOnDraw(); } } void UAirBlueprintLib::OnBeginPlay() { - FlushOnDrawCount = 0; + flush_on_draw_count_ = 0; + image_wrapper_module_ = & FModuleManager::LoadModuleChecked(FName("ImageWrapper")); } void UAirBlueprintLib::OnEndPlay() { //nothing to do for now + image_wrapper_module_ = nullptr; +} + +IImageWrapperModule* UAirBlueprintLib::getImageWrapperModule() +{ + return image_wrapper_module_; } void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec) { - if (log_messages_hidden) + if (log_messages_hidden_) return; @@ -334,7 +344,7 @@ void UAirBlueprintLib::SetObjectStencilID(ALandscapeProxy* mesh, int object_id) template std::string UAirBlueprintLib::GetMeshName(T* mesh) { - switch(mesh_naming_method) + switch(mesh_naming_method_) { case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: if (mesh->GetOwner()) @@ -354,7 +364,7 @@ std::string UAirBlueprintLib::GetMeshName(T* mesh) template<> std::string UAirBlueprintLib::GetMeshName(USkinnedMeshComponent* mesh) { - switch(mesh_naming_method) + switch(mesh_naming_method_) { case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: if (mesh->GetOwner()) @@ -630,4 +640,48 @@ UClass* UAirBlueprintLib::LoadClass(const std::string& name) throw std::invalid_argument(msg); } return cls; -} \ No newline at end of file +} + +void UAirBlueprintLib::CompressImageArray(int32 width, int32 height, const TArray &src, TArray &dest) +{ + TArray MutableSrcData = src; + + // PNGs are saved as RGBA but FColors are stored as BGRA. An option to swap the order upon compression may be added at + // some point. At the moment, manually swapping Red and Blue + for (int32 Index = 0; Index < width*height; Index++) + { + uint8 TempRed = MutableSrcData[Index].R; + MutableSrcData[Index].R = MutableSrcData[Index].B; + MutableSrcData[Index].B = TempRed; + } + + FObjectThumbnail TempThumbnail; + TempThumbnail.SetImageSize(width, height); + TArray& ThumbnailByteArray = TempThumbnail.AccessImageData(); + + // Copy scaled image into destination thumb + int32 MemorySize = width*height * sizeof(FColor); + ThumbnailByteArray.AddUninitialized(MemorySize); + FMemory::Memcpy(ThumbnailByteArray.GetData(), MutableSrcData.GetData(), MemorySize); + + // Compress data - convert into a .png + CompressUsingImageWrapper(ThumbnailByteArray, width, height, dest);; +} + +bool UAirBlueprintLib::CompressUsingImageWrapper(const TArray& uncompressed, const int32 width, const int32 height, TArray& compressed) +{ + bool bSucceeded = false; + compressed.Reset(); + if (uncompressed.Num() > 0) + { + IImageWrapperModule* ImageWrapperModule = UAirBlueprintLib::getImageWrapperModule(); + TSharedPtr ImageWrapper = ImageWrapperModule->CreateImageWrapper(EImageFormat::PNG); + if (ImageWrapper.IsValid() && ImageWrapper->SetRaw(&uncompressed[0], uncompressed.Num(), width, height, ERGBFormat::RGBA, 8)) + { + compressed = ImageWrapper->GetCompressed(); + bSucceeded = true; + } + } + + return bSucceeded; +} diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 3ecb5af9da..ee799396b6 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -14,6 +14,7 @@ #include "Components/MeshComponent.h" #include "LandscapeProxy.h" #include "common/AirSimSettings.hpp" +#include "IImageWrapperModule.h" #include "AirBlueprintLib.generated.h" @@ -91,15 +92,15 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static bool getLogMessagesHidden() { - return log_messages_hidden; + return log_messages_hidden_; } static void setLogMessagesHidden(bool is_hidden) { - log_messages_hidden = is_hidden; + log_messages_hidden_ = is_hidden; } static void SetMeshNamingMethod(msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType method) { - mesh_naming_method = method; + mesh_naming_method_ = method; } static void enableWorldRendering(AActor* context, bool enable); @@ -112,6 +113,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static UClass* LoadClass(const std::string& name); static void setUnrealClockSpeed(const AActor* context, float clock_speed); + static IImageWrapperModule* getImageWrapperModule(); + static void CompressImageArray(int32 width, int32 height, const TArray &src, TArray &dest); private: template @@ -126,11 +129,14 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void SetObjectStencilID(T* mesh, int object_id); static void SetObjectStencilID(ALandscapeProxy* mesh, int object_id); + static bool CompressUsingImageWrapper(const TArray& uncompressed, const int32 width, const int32 height, TArray& compressed); private: - static bool log_messages_hidden; + static bool log_messages_hidden_; //FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves - static uint32_t FlushOnDrawCount; - static msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType mesh_naming_method; + static uint32_t flush_on_draw_count_; + static msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType mesh_naming_method_; + + static IImageWrapperModule* image_wrapper_module_; }; diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 1f9e06087a..2f946af7e6 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -86,6 +86,8 @@ void ASimModeCar::continueForTime(double seconds) void ASimModeCar::setupClockSpeed() { + ASimModeBase::setupClockSpeed(); //do the default setup, //TODO: may be we want explicit scalable clock? + current_clockspeed_ = getSettings().clock_speed; //setup clock in PhysX diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp index 27767b10f8..7379dc2ee6 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp @@ -3,6 +3,7 @@ #include "Engine/TextureRenderTarget2D.h" #include "TaskGraphInterfaces.h" #include "ImageUtils.h" +#include "AirBlueprintLib.h" RenderRequest::RenderRequest(bool use_safe_method) : use_safe_method_(use_safe_method), params_(nullptr), results_(nullptr), req_size_(0), @@ -71,7 +72,7 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v if (results[i]->width != 0 && results[i]->height != 0) { results[i]->image_data_uint8.SetNumUninitialized(results[i]->width * results[i]->height * 4, false); if (params[i]->compress) - FImageUtils::CompressImageArray(results[i]->width, results[i]->height, results[i]->bmp, results[i]->image_data_uint8); + UAirBlueprintLib::CompressImageArray(results[i]->width, results[i]->height, results[i]->bmp, results[i]->image_data_uint8); else { uint8* ptr = results[i]->image_data_uint8.GetData(); for (const auto& item : results[i]->bmp) { diff --git a/build.cmd b/build.cmd index 3fbc798d1b..d565829cce 100644 --- a/build.cmd +++ b/build.cmd @@ -56,8 +56,8 @@ REM //---------- Build rpclib ------------ ECHO Starting cmake to build rpclib... IF NOT EXIST external\rpclib\rpclib-2.2.1\build mkdir external\rpclib\rpclib-2.2.1\build cd external\rpclib\rpclib-2.2.1\build -REM cmake -G"Visual Studio 15 2017 Win64" .. -cmake -G"Visual Studio 14 2015 Win64" .. +REM cmake -G"Visual Studio 14 2015 Win64" .. +cmake -G"Visual Studio 15 2017 Win64" .. cmake --build . cmake --build . --config Release if ERRORLEVEL 1 goto :buildfailed diff --git a/build.sh b/build.sh index cb454955dd..9087b2b0b9 100755 --- a/build.sh +++ b/build.sh @@ -37,13 +37,13 @@ else if [ "$(uname)" == "Darwin" ]; then CMAKE="$(greadlink -f cmake_build/bin/cmake)" - export CC=/usr/local/opt/llvm\@3.9/bin/clang - export CXX=/usr/local/opt/llvm\@3.9/bin/clang++ + export CC=/usr/local/opt/llvm\@5.0/bin/clang + export CXX=/usr/local/opt/llvm\@5.0/bin/clang++ else CMAKE="$(readlink -f cmake_build/bin/cmake)" - export CC="clang-3.9" - export CXX="clang++-3.9" + export CC="clang-5.0" + export CXX="clang++-5.0" fi fi diff --git a/setup.sh b/setup.sh index 06d855fc1b..8fee54bb2d 100755 --- a/setup.sh +++ b/setup.sh @@ -14,21 +14,21 @@ fi #give user perms to access USB port - this is not needed if not using PX4 HIL #TODO: figure out how to do below in travis -if [ "$(uname)" == "Darwin" ]; then +if [ "$(uname)" == "Darwin" ]; then # osx if [[ ! -z "${whoami}" ]]; then #this happens when running in travis sudo dseditgroup -o edit -a `whoami` -t user dialout fi #below takes way too long # brew install llvm@3.9 - brew install --force-bottle llvm@3.9 + brew install --force-bottle llvm@5.0 brew install wget brew install coreutils - export C_COMPILER=/usr/local/opt/llvm\@3.9/bin/clang - export COMPILER=/usr/local/opt/llvm\@3.9/bin/clang++ -else + export C_COMPILER=/usr/local/opt/llvm\@5.09/bin/clang + export COMPILER=/usr/local/opt/llvm\@5.0/bin/clang++ +else #linux if [[ ! -z "${whoami}" ]]; then #this happens when running in travis sudo /usr/sbin/useradd -G dialout $USER sudo usermod -a -G dialout $USER @@ -38,14 +38,14 @@ else sudo apt-get install -y build-essential wget -O - http://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - sudo apt-get update - sudo apt-get install -y clang-3.9 clang++-3.9 + sudo apt-get install -y clang-5.0 clang++-5.0 sudo apt-get install -y unzip - export C_COMPILER=clang-3.9 - export COMPILER=clang++-3.9 + export C_COMPILER=clang-5.0 + export COMPILER=clang++-5.0 fi -#download cmake - we need v3.9+ which is not available in Ubuntu 16.04 +#download cmake - we need v3.9+ which is not out of box in Ubuntu 16.04 if [[ ! -d "cmake_build/bin" ]]; then echo "Downloading cmake..." wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ @@ -121,12 +121,12 @@ fi # #sudo apt-get install -y clang-3.9-doc libclang-common-3.9-dev libclang-3.9-dev libclang1-3.9 libclang1-3.9-dbg libllvm-3.9-ocaml-dev libllvm3.9 libllvm3.9-dbg lldb-3.9 llvm-3.9 llvm-3.9-dev llvm-3.9-doc llvm-3.9-examples llvm-3.9-runtime clang-format-3.9 python-clang-3.9 libfuzzer-3.9-dev #get libc++ source -if [[ ! -d "llvm-source-39" ]]; then - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/llvm.git llvm-source-39 - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/libcxx.git llvm-source-39/projects/libcxx - git clone --depth=1 -b release_39 https://github.com/llvm-mirror/libcxxabi.git llvm-source-39/projects/libcxxabi +if [[ ! -d "llvm-source-50" ]]; then + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/llvm.git llvm-source-50 + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxx.git llvm-source-50/projects/libcxx + git clone --depth=1 -b release_50 https://github.com/llvm-mirror/libcxxabi.git llvm-source-50/projects/libcxxabi else - echo "folder llvm-source already exists, skipping git clone..." + echo "folder llvm-source-50 already exists, skipping git clone..." fi #build libc++ @@ -142,7 +142,7 @@ pushd llvm-build >/dev/null "$CMAKE" -DCMAKE_C_COMPILER=${C_COMPILER} -DCMAKE_CXX_COMPILER=${COMPILER} \ -LIBCXX_ENABLE_EXPERIMENTAL_LIBRARY=OFF -DLIBCXX_INSTALL_EXPERIMENTAL_LIBRARY=OFF \ -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=./output \ - ../llvm-source-39 + ../llvm-source-50 make cxx From a969762830a310ed3a3cf4c86939efa81d42bdff Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 18:32:12 -0700 Subject: [PATCH 112/121] fix warnings from new compiler --- AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp | 1 + MavLinkCom/include/MavLinkLog.hpp | 3 ++- UnrealPluginFiles.vcxproj | 6 +++--- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp index ea47702522..393a3b7ec3 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp @@ -476,6 +476,7 @@ class MultirotorApi : public VehicleApiBase { virtual void executeImpl(DroneControllerBase* controller, CancelableBase& cancelable) override { controller->moveOnPath(path_, velocity_, drivetrain_, yaw_mode_, lookahead_, adaptive_lookahead_, cancelable); } + virtual ~MoveOnPath() = default; }; diff --git a/MavLinkCom/include/MavLinkLog.hpp b/MavLinkCom/include/MavLinkLog.hpp index 731fd0d393..63a51f54b2 100644 --- a/MavLinkCom/include/MavLinkLog.hpp +++ b/MavLinkCom/include/MavLinkLog.hpp @@ -16,6 +16,7 @@ namespace mavlinkcom { public: virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) = 0; + virtual ~MavLinkLog() = default; }; // This implementation of MavLinkLog reads/writes MavLinkMessages to a local file. @@ -28,7 +29,7 @@ namespace mavlinkcom bool json_; public: MavLinkFileLog(); - ~MavLinkFileLog(); + virtual ~MavLinkFileLog(); bool isOpen(); void openForReading(const std::string& filename); void openForWriting(const std::string& filename, bool json = false); diff --git a/UnrealPluginFiles.vcxproj b/UnrealPluginFiles.vcxproj index 5321fbc9e5..893436cdc0 100644 --- a/UnrealPluginFiles.vcxproj +++ b/UnrealPluginFiles.vcxproj @@ -1,5 +1,5 @@  - + Debug @@ -36,12 +36,12 @@ Application true - v140 + v141 Application false - v140 + v141 From d36e897ff78e232f39a237df3b40ff3c38ea1e09 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 21:05:41 -0700 Subject: [PATCH 113/121] updated docs for upgrade --- docs/build_linux.md | 24 +++---------------- docs/build_windows.md | 22 +++++------------ docs/unreal_custenv.md | 4 ++-- docs/unreal_upgrade.md | 54 ++++++++++++++++++++++++++---------------- 4 files changed, 45 insertions(+), 59 deletions(-) diff --git a/docs/build_linux.md b/docs/build_linux.md index b11500e8db..d97b721116 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -10,10 +10,8 @@ It's super simple: 1-2-3! 2. Clone Unreal in your favorite folder and build it (this may take a while!). **Note**: We only support Unreal 4.17 at present. ```bash # go to the folder where you clone GitHub projects - git clone -b 4.17 https://github.com/EpicGames/UnrealEngine.git + git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git cd UnrealEngine - # the Unreal build was broken a few times so we will get the commit that works - git checkout af96417313a908b20621a443175ba91683c238c8 ./Setup.sh ./GenerateProjectFiles.sh make @@ -50,18 +48,6 @@ If this is the case then look for *.gch file(s) that follows after that message, If you see other compile errors in console then open up those source files and see if it is due to changes you made. If not, then report it as issue on GitHub. -#### What are the known issues with Unreal 4.16? - -* One of the major issues is [this bug in Unreal](https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html). We have a workaround for some parts of the code but we haven't tested if everything is covered. -* Clicking the "End" button causes Unreal to crash. -* The report function (when you press R) also causes a crash because of the above reasons. - -#### What are the known issues with Unreal 4.17? - -* We have seen some random crashes during the startup. -* You might get a warning that says that the AirSim plugin is incompatible, which you can ignore. -* Clicking the "End" button freezes the Unreal Editor. When this happens you will need to manually kill the process. - #### Unreal crashed! How do I know what went wrong? Go to the `MyUnrealProject/Saved/Crashes` folder and search for the file `MyProject.log` within its subdirectories. At the end of this file you will see the stack trace and messages. You can also take a look at the `Diagnostics.txt` file. @@ -76,15 +62,11 @@ Yes, you can, but we haven't tested it. You can find the instructions [here](htt #### What compiler and stdlib does AirSim use? -We use the same compiler, **Clang 3.9**, and stdlib, **libc++**, that Unreal uses. AirSim's `setup.sh` will automatically download them both. The libc++ source code is cloned into the `llvm-source` folder and is built into the `llvm-build` folder, from where CMake uses libc++. - -#### Can I use AirSim with Unreal 4.16? - -Yes! The `*.Build.cs` files are, however, no longer compatible (you will get a compile error). You can find files for 4.16 as `*.Build.4.16.cs` so just rename those. +We use the same compiler that Unreal Engine uses, **Clang 5.0**, and stdlib, **libc++**. AirSim's `setup.sh` will automatically download them both. The libc++ source code is cloned into the `llvm-source-(version)` folder and is built into the `llvm-build` folder, from where CMake uses libc++. #### What version of CMake does the AirSim build use? -3.5.0 or higher. This should be the default in Ubuntu 16.04. You can check your CMake version using `cmake --version`. If you have an older version, follow [these instructions](cmake_linux.md) or see the [CMake website](https://cmake.org/install/). +3.9.0 or higher. This is *not* the default in Ubuntu 16.04 so setup.sh installs it for you. You can check your CMake version using `cmake --version`. If you have an older version, follow [these instructions](cmake_linux.md) or see the [CMake website](https://cmake.org/install/). #### Can I compile AirSim in BashOnWindows? diff --git a/docs/build_windows.md b/docs/build_windows.md index f22e7f9bca..8967206b63 100644 --- a/docs/build_windows.md +++ b/docs/build_windows.md @@ -3,18 +3,17 @@ ## Install Unreal Engine 1. [Download](https://www.unrealengine.com/download) the Epic Games Launcher. While the Unreal Engine is open source and free to download, registration is still required. -2. Run the Epic Games Launcher, open the Library tab from left, click on the "Add Versions" which should show the option to download Unreal 4.16 as shown below. If you have multiple versions of Unreal installed then make sure 4.16 is "Current" by clicking down arrow next to the Launch button for the version. +2. Run the Epic Games Launcher, open the Library tab from left, click on the "Add Versions" which should show the option to download Unreal 4.18 as shown below. If you have multiple versions of Unreal installed then make sure 4.18 is "Current" by clicking down arrow next to the Launch button for the version. - **Note**: Older versions of Unreal are not supported. Please see the [upgrade guide](unreal_upgrade.md) to upgrade your projects. + **Note**: If you have UE 4.16 or older projects, please see the [upgrade guide](unreal_upgrade.md) to upgrade your projects. ![Unreal Versions](images/unreal_versions.png) ## Build AirSim - 1. You will need Visual Studio 2015 Update 3 (make sure to install VC++) or newer. Other versions haven't been tested. - 2. Start VS2015 x64 Native Tools Command Prompt. Create a folder for the repo and run `git clone https://github.com/Microsoft/AirSim.git`. - 3. Install [CMake](https://cmake.org/download/) which is used to build the rpclib submodule. - 4. Run `build.cmd` from the command line. This will create ready to use plugin bits in the `Unreal\Plugins` folder that can be dropped into any Unreal project. + 1. You will need Visual Studio 2017 (make sure to install VC++) or newer. + 2. Start `x64 Native Tools Command Prompt for VS 2017`. Create a folder for the repo and run `git clone https://github.com/Microsoft/AirSim.git`. + 3. Run `build.cmd` from the command line. This will create ready to use plugin bits in the `Unreal\Plugins` folder that can be dropped into any Unreal project. ## Setup Remote Control @@ -31,15 +30,6 @@ Finally, you will need an Unreal project that hosts the environment for your veh #### How do I use PX4 firmware with AirSim? By default, AirSim uses its own built-in firmware called [simple_flight](simple_flight.md). There is no additional setup if you just want to go with it. If you want to switch to using PX4 instead then please see [this guide](px4_setup.md). -#### Build is not working on VS 2017 -Known working config is: -```` -Windows 10 (Education) x64 -VS2015 update 3 (x86) with VC++ -Cmake 3.9 (x86) -```` -Even though cmake 3.7 says it added support for VS 2017 folks are reporting build issues with that. - #### I made changes in Visual Studio but there is no effect -Sometimes the Unreal + VS build system doesn't recompile if you make changes to only header files. To ensure a recompile, make some cpp file "dirty". +Sometimes the Unreal + VS build system doesn't recompile if you make changes to only header files. To ensure a recompile, make some Unreal based cpp file "dirty" like AirSimGameMode.cpp. diff --git a/docs/unreal_custenv.md b/docs/unreal_custenv.md index 2c836fbf83..e64ad447f3 100644 --- a/docs/unreal_custenv.md +++ b/docs/unreal_custenv.md @@ -8,7 +8,7 @@ There is no `Epic Games Launcher` for Linux which means that if you need to crea ## Step by Step Instructions -1. Make sure AirSim is built and Unreal 4.16 is installed as described in [build instructions](build_windows.md). +1. Make sure AirSim is built and Unreal 4.18 is installed as described in [build instructions](build_windows.md). 2. In `Epic Games Launcher` click the Learn tab then scroll down and find `Landscape Mountains`. Click the `Create Project` and download this content (~2GB download). ![current version](images/landscape_mountains.png) @@ -26,7 +26,7 @@ There is no `Epic Games Launcher` for Linux which means that if you need to crea ``` { "FileVersion": 3, - "EngineAssociation": "4.16", + "EngineAssociation": "4.18", "Category": "Samples", "Description": "", "Modules": [ diff --git a/docs/unreal_upgrade.md b/docs/unreal_upgrade.md index 0cf7ad7fba..ad5aa3c9c7 100644 --- a/docs/unreal_upgrade.md +++ b/docs/unreal_upgrade.md @@ -1,35 +1,49 @@ -# Upgrading Unreal Engine Version +# Upgrading to UE 4.18 and Visual Studio 2017 -## Blocks Project +These instructions applies if you are already using AirSim on Unreal Engine 4.16. If you never installed AirSim, please see [How to get it](https://github.com/microsoft/airsim#how-to-get-it). -If you are using Blocks project that comes with AirSim then you don't need to do anything other than [installling Unreal 4.16](build_windows.md). +## Do this first -## Your Own Unreal Project -If you have your own Unreal project created in version Unreal 4.15 then you need to upgrade your project to use Unreal 4.16. +### For Windows Users +1. Install Visual Studio 2017 with VC++, Python and C#. +2. Install UE 4.18 through Epic Games Launcher. +3. Start `x64 Native Tools Command Prompt for VS 2017` and navigate to AirSim repo. +4. Run `git clean -fdx` to remove all unchecked/extra stuff from your repo. +5. Run `git pull` to get latest code from GitHub. +6. Run `build.cmd` to build everything. -### Option 1: Just Recreate Project +### For Linux Users +1. Rename or delete your exiting folder for Unreal 4.17. +2. Rename or delete your existing folder for AirSim repo. +2. Follow [Install steps for Linux](https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md#install-and-build). -If your project doesn't have any code or assets other than environment you downloaded then you can also simply [recreate the project in Unreal 4.16 Editor](unreal_custenv.md) and then copy Plugins folder from `AirSim/Unreal/Plugins`. +## Upgrading Your Custom Unreal Project +If you have your own Unreal project created in older version of Unreal Engine then you need to upgrade your project to Unreal 4.18. To do this, -### Option 2: Modify Few Files +1. Open .uproject file and look for the line `"EngineAssociation"` and make sure it reads like `"EngineAssociation": "4.18"`. +2. Delete `Plugins/AirSim` folder in your Unreal project's folder. +3. Go to your AirSim repo folder and copy `Unreal\Plugins` folder to your Unreal project's folder. +4. Copy *.bat (or *.sh for Linux) from `Unreal\Environments\Blocks` to your project's folder. +5. Run `clean.bat` (or `clean.sh` for Linux) followed by `GenerateProjectFiles.bat` (only for Windows). -Unreal 4.16 Build system has breaking changes. So you need to modify your *.Build.cs and *.Target.cs which you can find in `Source` folder of your Unreal project. So what are those changes? Below is the gist of it but you should really refer to [Unreal's official 4.16 transition post](https://forums.unrealengine.com/showthread.php?145757-C-4-16-Transition-Guide). +## FAQ -#### In your project's *.Target.cs +### I have Unreal project that is older than 4.16. How do I upgrade it? + +#### Option 1: Just Recreate Project +If your project doesn't have any code or assets other than environment you downloaded then you can also simply [recreate the project in Unreal 4.18 Editor](unreal_custenv.md) and then copy Plugins folder from `AirSim/Unreal/Plugins`. + +#### Option 2: Modify Few Files +Unreal versions newer than Unreal 4.15 has breaking changes. So you need to modify your *.Build.cs and *.Target.cs which you can find in `Source` folder of your Unreal project. So what are those changes? Below is the gist of it but you should really refer to [Unreal's official 4.16 transition post](https://forums.unrealengine.com/showthread.php?145757-C-4-16-Transition-Guide). + +##### In your project's *.Target.cs 1. Change the contructor from, `public MyProjectTarget(TargetInfo Target)` to `public MyProjectTarget(TargetInfo Target) : base(Target)` 2. Remove `SetupBinaries` method if you have one and instead add following line in contructor above: `ExtraModuleNames.AddRange(new string[] { "MyProject" });` -#### In your project's *.Build.cs +##### In your project's *.Build.cs Change the constructor from `public MyProject(TargetInfo Target)` to `public MyProject(ReadOnlyTargetRules Target) : base(Target)`. -#### In your *.uproject -Remove line for `EngineAssociation` - -#### And finally... -1. Make sure [Unreal 4.16 is installed](build,md). -2. Double click on your project's `*.uproject` file. -3. If you are asked to select Unreal version, select 4.16. -4. The warning box might show only "Open Copy" button. Don't click that. Instead click on More Options which will reveal more buttons. Choose Convert-In-Place option. Causion: Always keep backup of your project first! -5. If you don't have anything nasty, in place conversion should go through and you are now on new version of Unreal! +##### And finally... +Follow above steps to continue upgrade. The warning box might show only "Open Copy" button. Don't click that. Instead click on More Options which will reveal more buttons. Choose `Convert-In-Place option`. *Causion:* Always keep backup of your project first! If you don't have anything nasty, in place conversion should go through and you are now on new version of Unreal. From 58a20b0633fe4ba16a8522e3cfd8f96694a22b0c Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 21:40:11 -0700 Subject: [PATCH 114/121] added check for old versions for existing users --- build.cmd | 20 ++++++++++++++++++-- build.sh | 12 ++++++++++++ setup.sh | 7 +++++++ 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/build.cmd b/build.cmd index d565829cce..244f8578e9 100644 --- a/build.cmd +++ b/build.cmd @@ -6,6 +6,20 @@ set ROOT_DIR=%~dp0 REM // Check command line arguments set "noFullPolyCar=" +REM //check VS version +if "%VisualStudioVersion%"=="" ( + echo( + echo oh oh... You need to run this command from x64 Native Tools Command Prompt for VS 2017. + goto :buildfailed_nomsg +) +if "%VisualStudioVersion%"=="14.0" ( + echo( + echo Hello there! We just upgraded AirSim to Unreal Engine 4.18 and Visual Studio 2017. + echo Here are few easy steps for upgrade so everything is new and shiny: + echo https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md + goto :buildfailed_nomsg +) + if "%1"=="" goto noargs if "%1"=="--no-full-poly-car" set "noFullPolyCar=y" @@ -19,7 +33,7 @@ if ERRORLEVEL 1 ( CALL check_cmake.bat if ERRORLEVEL 1 ( echo( - echo ERROR: cmake was not installed correctly. + echo ERROR: cmake was not installed correctly, we tried. goto :buildfailed ) ) @@ -141,9 +155,11 @@ REM //---------- done building ---------- exit /b 0 :buildfailed -chdir /d %ROOT_DIR% echo( echo #### Build failed - see messages above. 1>&2 + +:buildfailed_nomsg +chdir /d %ROOT_DIR% exit /b 1 diff --git a/build.sh b/build.sh index 9087b2b0b9..ef5a66a119 100755 --- a/build.sh +++ b/build.sh @@ -7,6 +7,18 @@ pushd "$SCRIPT_DIR" >/dev/null set -e # set -x +#check for correct verion of llvm +if [[ ! -d "llvm-source-50" ]]; then + if [[ -d "llvm-source-39" ]]; then + echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." + echo "Here are few easy steps for upgrade so everything is new and shiny :)" + echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" + exit 1 + else + echo "The llvm-souce-50 folder was not found! Mystery indeed."" + fi +fi + # check for libc++ if [[ !(-d "./llvm-build/output/lib") ]]; then echo "ERROR: clang++ and libc++ is necessary to compile AirSim and run it in Unreal engine" diff --git a/setup.sh b/setup.sh index 8fee54bb2d..cac0ca343f 100755 --- a/setup.sh +++ b/setup.sh @@ -3,6 +3,13 @@ set -x set -e +if [[ -d "llvm-source-39" ]]; then + echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." + echo "Here are few easy steps for upgrade so everything is new and shiny :)" + echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" + exit 1 +fi + SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" pushd "$SCRIPT_DIR" >/dev/null From 4ede21d1e5afcf117915e3bbfafe421a676c15dd Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 22:38:47 -0700 Subject: [PATCH 115/121] Better Linux upgrade steps --- build.sh | 2 +- clean_rebuild.sh | 15 +++++++++++++++ docs/unreal_upgrade.md | 9 ++++----- install_run_all.sh | 2 +- install_unreal.sh | 34 ++++++++++++++++++++++++++++++++++ setup.sh | 6 +++--- 6 files changed, 58 insertions(+), 10 deletions(-) create mode 100755 clean_rebuild.sh create mode 100755 install_unreal.sh diff --git a/build.sh b/build.sh index ef5a66a119..fa90fbca43 100755 --- a/build.sh +++ b/build.sh @@ -15,7 +15,7 @@ if [[ ! -d "llvm-source-50" ]]; then echo "https://github.com/Microsoft/AirSim/blob/master/docs/unreal_upgrade.md" exit 1 else - echo "The llvm-souce-50 folder was not found! Mystery indeed."" + echo "The llvm-souce-50 folder was not found! Mystery indeed." fi fi diff --git a/clean_rebuild.sh b/clean_rebuild.sh new file mode 100755 index 0000000000..886e594424 --- /dev/null +++ b/clean_rebuild.sh @@ -0,0 +1,15 @@ +#! /bin/bash + +# get path of current script: https://stackoverflow.com/a/39340259/207661 +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +pushd "$SCRIPT_DIR" >/dev/null + +set -x + +git clean -ffdx +git pull + +set -e + +./setup.sh +./build.sh \ No newline at end of file diff --git a/docs/unreal_upgrade.md b/docs/unreal_upgrade.md index ad5aa3c9c7..949de0d3e6 100644 --- a/docs/unreal_upgrade.md +++ b/docs/unreal_upgrade.md @@ -1,5 +1,4 @@ - -# Upgrading to UE 4.18 and Visual Studio 2017 +# Upgrading to Unreal Engine 4.18 These instructions applies if you are already using AirSim on Unreal Engine 4.16. If you never installed AirSim, please see [How to get it](https://github.com/microsoft/airsim#how-to-get-it). @@ -14,9 +13,9 @@ These instructions applies if you are already using AirSim on Unreal Engine 4.16 6. Run `build.cmd` to build everything. ### For Linux Users -1. Rename or delete your exiting folder for Unreal 4.17. -2. Rename or delete your existing folder for AirSim repo. -2. Follow [Install steps for Linux](https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md#install-and-build). +1. From your AirSim repo folder, run 'clean_rebuild.sh`. +2. Rename or delete your exiting folder for Unreal Engine. +3. Follow step 1 and 2 to [install Unreal Engine 4.18](https://github.com/Microsoft/AirSim/blob/master/docs/build_linux.md#install-and-build). ## Upgrading Your Custom Unreal Project If you have your own Unreal project created in older version of Unreal Engine then you need to upgrade your project to Unreal 4.18. To do this, diff --git a/install_run_all.sh b/install_run_all.sh index 9214a19225..4293bf4d94 100755 --- a/install_run_all.sh +++ b/install_run_all.sh @@ -23,7 +23,7 @@ fi #install unreal if [[ !(-d "$UnrealDir") ]]; then - git clone -b 4.17 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" + git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" pushd "$UnrealDir" >/dev/null ./Setup.sh diff --git a/install_unreal.sh b/install_unreal.sh new file mode 100755 index 0000000000..41e145486c --- /dev/null +++ b/install_unreal.sh @@ -0,0 +1,34 @@ +#! /bin/bash + +# get path of current script: https://stackoverflow.com/a/39340259/207661 +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +pushd "$SCRIPT_DIR" >/dev/null + +set -e +set -x + +#confirm unreal install directory +UnrealDir=$1 +if [[ !(-z "UnrealDir") ]]; then + UnrealDir="$SCRIPT_DIR/UnrealEngine" +fi + +read -p "Unreal will be installed in $UnrealDir. To change it invoke script with path argument. Continue? " -n 1 -r +echo +if [[ ! $REPLY =~ ^[Yy]$ ]] +then + popd >/dev/null + exit 0 +fi + +#install unreal +if [[ !(-d "$UnrealDir") ]]; then + git clone -b 4.18 https://github.com/EpicGames/UnrealEngine.git "$UnrealDir" + pushd "$UnrealDir" >/dev/null + + ./Setup.sh + ./GenerateProjectFiles.sh + make + + popd >/dev/null +fi \ No newline at end of file diff --git a/setup.sh b/setup.sh index cac0ca343f..3662d4bd6b 100755 --- a/setup.sh +++ b/setup.sh @@ -1,8 +1,5 @@ #! /bin/bash -set -x -set -e - if [[ -d "llvm-source-39" ]]; then echo "Hello there! We just upgraded AirSim to Unreal Engine 4.18." echo "Here are few easy steps for upgrade so everything is new and shiny :)" @@ -10,6 +7,9 @@ if [[ -d "llvm-source-39" ]]; then exit 1 fi +set -x +set -e + SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" pushd "$SCRIPT_DIR" >/dev/null From 77194b0169fa382e2f22b2dd98543675afe1d4e3 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Fri, 27 Apr 2018 23:15:01 -0700 Subject: [PATCH 116/121] less steps for Windows upgrade --- clean_rebuild.bat | 8 ++++++++ docs/unreal_upgrade.md | 6 +++--- 2 files changed, 11 insertions(+), 3 deletions(-) create mode 100644 clean_rebuild.bat diff --git a/clean_rebuild.bat b/clean_rebuild.bat new file mode 100644 index 0000000000..4af6112702 --- /dev/null +++ b/clean_rebuild.bat @@ -0,0 +1,8 @@ +@echo off +REM //---------- set up variable ---------- +setlocal +set ROOT_DIR=%~dp0 + +git clean -ffdx +git pull +build.cmd diff --git a/docs/unreal_upgrade.md b/docs/unreal_upgrade.md index 949de0d3e6..5d7588de34 100644 --- a/docs/unreal_upgrade.md +++ b/docs/unreal_upgrade.md @@ -2,15 +2,15 @@ These instructions applies if you are already using AirSim on Unreal Engine 4.16. If you never installed AirSim, please see [How to get it](https://github.com/microsoft/airsim#how-to-get-it). +**Caution:** Below steps will delete your any unsaved work in AirSim or Unreal folder. + ## Do this first ### For Windows Users 1. Install Visual Studio 2017 with VC++, Python and C#. 2. Install UE 4.18 through Epic Games Launcher. 3. Start `x64 Native Tools Command Prompt for VS 2017` and navigate to AirSim repo. -4. Run `git clean -fdx` to remove all unchecked/extra stuff from your repo. -5. Run `git pull` to get latest code from GitHub. -6. Run `build.cmd` to build everything. +4. Run `clean_rebuild.bat` to remove all unchecked/extra stuff and rebuild everything. ### For Linux Users 1. From your AirSim repo folder, run 'clean_rebuild.sh`. From 64ebb51a62fca08c951e575b5b66a2be4635ed13 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Sat, 28 Apr 2018 00:08:19 -0700 Subject: [PATCH 117/121] updated what's new list --- README.md | 11 +++---- docs/whats_new.md | 75 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+), 7 deletions(-) create mode 100644 docs/whats_new.md diff --git a/README.md b/README.md index e8c7bb3d08..816e77f8ba 100644 --- a/README.md +++ b/README.md @@ -73,12 +73,9 @@ Yet another way to use AirSim is so-called "Computer Vision" mode. In this mode, ## What's New -* We now have the [car model](docs/using_car.md). -* No need to build the code. Just download [binaries](https://github.com/Microsoft/AirSim/releases) and you are good to go! -* The [reinforcement learning example](docs/reinforcement_learning.md) with AirSim -* New built-in flight controller called [simple_flight](docs/simple_flight.md) that "just works" without any additional setup. It is also now *default*. -* AirSim now also generates [depth as well as disparity images](docs/image_apis.md) that is in camera plan. -* We also have official Linux build now! If you have been using AirSim with PX4, you might want to read the [release notes](docs/release_notes.md). +* We have upgraded to Unreal Engine 4.18 and Visual Studio 2017 (see [upgrade instructions](docs/unreal_upgrade.md)) + +[List of newly added features](docs/whats_new.md) ## Participate @@ -98,7 +95,7 @@ More technical details are available in [AirSim paper (FSR 2017 Conference)](htt ### Contribute -Please take a look at [open issues](https://github.com/microsoft/airsim/issues) and [Trello board](https://trello.com/b/1t2qCeaA/todo) if you are looking for areas to contribute to. +Please take a look at [open issues](https://github.com/microsoft/airsim/issues) if you are looking for areas to contribute to. * [More on AirSim design](docs/design.md) * [More on code structure](docs/code_structure.md) diff --git a/docs/whats_new.md b/docs/whats_new.md new file mode 100644 index 0000000000..033b7676bd --- /dev/null +++ b/docs/whats_new.md @@ -0,0 +1,75 @@ +# What's new + +Below is highly summerized curated list of important changes. This does not include minor/less important changes or bug fixes or things like documentation update. This list updated every few months. For full list of changes, please review [commit history](https://github.com/Microsoft/AirSim/commits/master). + + +### April, 2018 +* Upgraded to Unreal Engine 4.18 and Visual Studio 2017 +* API framework refactoring to support world-level APIs +* Latest PX4 firmware now supported +* CarState with more information +* ThrustMaster wheel support +* pause and continueForTime APIs for drone as well as car +* Allow drone simulation run at higher clock rate without any degradation +* Forward-only mode fully functional for drone (do orbits while looking at center) +* Better PID tuning to reduce wobble for drones +* Ability to set arbitrary vehicle blueprint for drone as well as car +* gimbal stabilization via settings +* Ability to segment skinned and skeletal meshes by their name +* moveByAngleThrottle API +* Car physics tuning for better maneuverability +* Configure additional cameras via settings +* Time of day with geographically computed sun position +* Better car steering via keyboard +* Added MeshNamingMethod in segmentation setting +* gimbal API +* getCameraParameters API +* Ability turn off main rendering to save GPU resources +* Projection mode for capture settings +* getRCData, setRCData APIs +* Ability to turn off segmentation using negative IDs +* OSX build improvements +* Segmentation working for very large environments with initial IDs +* Better and extensible hash calculation for segmentation IDs +* Extensible PID controller for custom integration methods +* Sensor architecture now enables renderer specific features like ray casting +* Laser altimeter sensor + + +### Jan 2018 +* Config system rewrite, enable flexible config we are targeting in future +* Multi-Vehicle support Phase 1, core infrastructure changes +* MacOS support +* Infrared view +* 5 types of noise and interference for cameras +* WYSIWIG capture settings for cameras, preview recording settings in main view +* Azure support Phase 1, enable configurability of instances for headless mode +* Full kinematics APIs, ability to get pose, linear and angular velocities + accelerations via APIs +* Record multiple images from multiple cameras +* New segmentation APIs, ability to set configure object IDs, search via regex +* New object pose APIs, ability to get pose of objects (like animals) in environment +* Camera infrastructure enhancements, ability to add new image types like IR with just few lines +* Clock speed APIs for drone as well as car, simulation can be run with speed factor of 0 < x < infinity +* Support for Logitech G920 wheel +* Physics tuning of the car, Car doesn’t roll over, responds to steering with better curve, releasing gas paddle behavior more realistic +* Debugging APIs +* Stress tested to 24+ hours of continuous runs +* Support for Landscape and sky segmentation +* Manual navigation with accelerated controls in CV mode, user can explore environment much more easily +* Collison APIs +* Recording enhancements, log several new data points including ground truth, multiple images, controls state +* Planner and Perspective Depth views +* Disparity view +* New Image APIs supports float, png or numpy formats +* 6 config settings for image capture, ability to set auto-exposure, motion blur, gamma etc +* Full multi-camera support through out including sub-windows, recording, APIs etc +* Command line script to build all environments in one shot +* Remove submodules, use rpclib as download + +### Nov 2017 +* We now have the [car model](docs/using_car.md). +* No need to build the code. Just download [binaries](https://github.com/Microsoft/AirSim/releases) and you are good to go! +* The [reinforcement learning example](docs/reinforcement_learning.md) with AirSim +* New built-in flight controller called [simple_flight](docs/simple_flight.md) that "just works" without any additional setup. It is also now *default*. +* AirSim now also generates [depth as well as disparity images](docs/image_apis.md) that is in camera plan. +* We also have official Linux build now! If you have been using AirSim with PX4, you might want to read the [release notes](docs/release_notes.md). \ No newline at end of file From a085125f8df51768765fcb854aaffc492eb98764 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Sat, 28 Apr 2018 00:11:10 -0700 Subject: [PATCH 118/121] Thrustmaster bug fix, https://github.com/Microsoft/AirSim/issues/1027 --- .../AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 1309a65439..575ef16dc0 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -80,8 +80,11 @@ struct DirectInputJoyStick::impl{ g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; - g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | - DIEP_TYPESPECIFICPARAMS | DIEP_START); + if (g_pAutoCenterHandle) { + g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | + DIEP_TYPESPECIFICPARAMS | DIEP_START); + } + } #define FFWRMAX 0.08 From 7bddd5857791f9c164e8eba80c229f199c0babf8 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Sun, 29 Apr 2018 00:53:05 -0700 Subject: [PATCH 119/121] fix several compile errors due to IWYU mode in UE 4.18 --- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 205 ++---------------- .../Plugins/AirSim/Source/AirBlueprintLib.h | 180 +++++++++++++-- Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp | 24 +- Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h | 2 +- .../Plugins/AirSim/Source/Car/SimModeCar.cpp | 10 +- Unreal/Plugins/AirSim/Source/Car/SimModeCar.h | 2 + .../AirSim/Source/ManualPoseController.h | 2 + .../Multirotor/SimModeWorldMultiRotor.cpp | 15 +- .../Multirotor/SimModeWorldMultiRotor.h | 2 + Unreal/Plugins/AirSim/Source/NedTransform.h | 3 +- Unreal/Plugins/AirSim/Source/PIPCamera.h | 2 + .../Source/Recording/RecordingThread.cpp | 3 +- .../AirSim/Source/Recording/RecordingThread.h | 3 +- .../Plugins/AirSim/Source/RenderRequest.cpp | 15 +- Unreal/Plugins/AirSim/Source/RenderRequest.h | 3 + .../Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 7 +- .../SimJoyStick/DirectInputJoyStick.cpp | 35 +-- .../AirSim/Source/UnrealImageCapture.cpp | 4 +- .../AirSim/Source/VehiclePawnWrapper.h | 4 +- clean.cmd | 1 + 20 files changed, 268 insertions(+), 254 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 0cef26c4b1..60794c488e 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -5,15 +5,12 @@ #include "GameFramework/WorldSettings.h" #include "Components/SceneCaptureComponent2D.h" #include "Components/SkinnedMeshComponent.h" -#include "Kismet/GameplayStatics.h" #include "GameFramework/RotatingMovementComponent.h" -#include -#include "common/common_utils/Utils.hpp" #include "Components/StaticMeshComponent.h" #include "EngineUtils.h" -#include "Runtime/Landscape/Classes/LandscapeComponent.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" #include "UObjectIterator.h" +#include "Camera/CameraComponent.h" //#include "Runtime/Foliage/Public/FoliageType.h" #include "Kismet/KismetStringLibrary.h" #include "MessageDialog.h" @@ -23,6 +20,8 @@ #include "IImageWrapper.h" #include "ObjectThumbnail.h" #include "Engine/Engine.h" +#include +#include "common/common_utils/Utils.hpp" /* //TODO: change naming conventions to same as other files? @@ -140,7 +139,7 @@ void UAirBlueprintLib::enableViewportRendering(AActor* context, bool enable) void UAirBlueprintLib::OnBeginPlay() { flush_on_draw_count_ = 0; - image_wrapper_module_ = & FModuleManager::LoadModuleChecked(FName("ImageWrapper")); + image_wrapper_module_ = &FModuleManager::LoadModuleChecked(FName("ImageWrapper")); } void UAirBlueprintLib::OnEndPlay() @@ -171,19 +170,19 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix, FColor color; switch (level) { - case LogDebugLevel::Informational: - color = FColor(147, 231, 237); + case LogDebugLevel::Informational: + color = FColor(147, 231, 237); //UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); break; - case LogDebugLevel::Success: - color = FColor(156, 237, 147); + case LogDebugLevel::Success: + color = FColor(156, 237, 147); //UE_LOG(LogAirSim, Log, TEXT("%s%s"), *prefix, *suffix); break; - case LogDebugLevel::Failure: + case LogDebugLevel::Failure: color = FColor(237, 147, 168); //UE_LOG(LogAirSim, Error, TEXT("%s%s"), *prefix, *suffix); break; - case LogDebugLevel::Unimportant: + case LogDebugLevel::Unimportant: color = FColor(237, 228, 147); //UE_LOG(LogAirSim, Verbose, TEXT("%s%s"), *prefix, *suffix); break; @@ -234,23 +233,7 @@ template UChildActorComponent* UAirBlueprintLib::GetActorComponent(AActor*, FStr template USceneCaptureComponent2D* UAirBlueprintLib::GetActorComponent(AActor*, FString); template UStaticMeshComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); template URotatingMovementComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); - -template -T* UAirBlueprintLib::FindActor(const UObject* context, FString name) -{ - TArray foundActors; - FindAllActor(context, foundActors); - FName name_n = FName(*name); - - for (AActor* actor : foundActors) { - if (actor->ActorHasTag(name_n) || actor->GetName().Compare(name) == 0) { - return static_cast(actor); - } - } - - //UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure); - return nullptr; -} +template UCameraComponent* UAirBlueprintLib::GetActorComponent(AActor*, FString); bool UAirBlueprintLib::IsInGameThread() { @@ -268,83 +251,10 @@ void UAirBlueprintLib::RunCommandOnGameThread(TFunction InFunction, bool } } - -template -void UAirBlueprintLib::FindAllActor(const UObject* context, TArray& foundActors) -{ - UGameplayStatics::GetAllActorsOfClass(context == nullptr ? GEngine : context, T::StaticClass(), foundActors); -} - -template -void UAirBlueprintLib::InitializeObjectStencilID(T* mesh, bool ignore_existing) -{ - std::string mesh_name = common_utils::Utils::toLower(GetMeshName(mesh)); - if (mesh_name == "" || common_utils::Utils::startsWith(mesh_name, "default_")) { - //common_utils::Utils::DebugBreak(); - return; - } - FString name(mesh_name.c_str()); - int hash = 5; - for (int idx = 0; idx < name.Len(); ++idx) { - auto char_num = UKismetStringLibrary::GetCharacterAsNumber(name, idx); - if (char_num < 97) - continue; //numerics and other punctuations - hash += char_num; - } - if (ignore_existing || mesh->CustomDepthStencilValue == 0) { //if value is already set then don't bother - SetObjectStencilID(mesh, hash % 256); - } -} - -template -void UAirBlueprintLib::SetObjectStencilID(T* mesh, int object_id) -{ - if (object_id < 0) - { - mesh->SetRenderCustomDepth(false); - } - else - { - mesh->SetCustomDepthStencilValue(object_id); - mesh->SetRenderCustomDepth(true); - } - //mesh->SetVisibility(false); - //mesh->SetVisibility(true); -} - -void UAirBlueprintLib::SetObjectStencilID(ALandscapeProxy* mesh, int object_id) -{ - if (object_id < 0) - { - mesh->bRenderCustomDepth = false; - } - else - { - mesh->CustomDepthStencilValue = object_id; - mesh->bRenderCustomDepth = true; - } - - // Explicitly set the custom depth state on the components so the - // render state is marked dirty and the update actually takes effect - // immediately. - for (ULandscapeComponent* comp : mesh->LandscapeComponents) - { - if (object_id < 0) - { - comp->SetRenderCustomDepth(false); - } - else - { - comp->SetCustomDepthStencilValue(object_id); - comp->SetRenderCustomDepth(true); - } - } -} - -template -std::string UAirBlueprintLib::GetMeshName(T* mesh) +template<> +std::string UAirBlueprintLib::GetMeshName(USkinnedMeshComponent* mesh) { - switch(mesh_naming_method_) + switch (mesh_naming_method_) { case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: if (mesh->GetOwner()) @@ -352,8 +262,8 @@ std::string UAirBlueprintLib::GetMeshName(T* mesh) else return ""; // std::string(TCHAR_TO_UTF8(*(UKismetSystemLibrary::GetDisplayName(mesh)))); case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::StaticMeshName: - if (mesh->GetStaticMesh()) - return std::string(TCHAR_TO_UTF8(*(mesh->GetStaticMesh()->GetName()))); + if (mesh->SkeletalMesh) + return std::string(TCHAR_TO_UTF8(*(mesh->SkeletalMesh->GetName()))); else return ""; default: @@ -361,26 +271,6 @@ std::string UAirBlueprintLib::GetMeshName(T* mesh) } } -template<> -std::string UAirBlueprintLib::GetMeshName(USkinnedMeshComponent* mesh) -{ - switch(mesh_naming_method_) - { - case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: - if (mesh->GetOwner()) - return std::string(TCHAR_TO_UTF8(*(mesh->GetOwner()->GetName()))); - else - return ""; // std::string(TCHAR_TO_UTF8(*(UKismetSystemLibrary::GetDisplayName(mesh)))); - case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::StaticMeshName: - if (mesh->SkeletalMesh) - return std::string(TCHAR_TO_UTF8(*(mesh->SkeletalMesh->GetName()))); - else - return ""; - default: - return ""; - } -} - std::string UAirBlueprintLib::GetMeshName(ALandscapeProxy* mesh) { @@ -407,20 +297,6 @@ void UAirBlueprintLib::InitializeMeshStencilIDs(bool ignore_existing) } } -template -void UAirBlueprintLib::SetObjectStencilIDIfMatch(T* mesh, int object_id, const std::string& mesh_name, bool is_name_regex, - const std::regex& name_regex, int& changes) -{ - std::string comp_mesh_name = GetMeshName(mesh); - if (comp_mesh_name == "") - return; - bool is_match = (!is_name_regex && (comp_mesh_name == mesh_name)) - || (is_name_regex && std::regex_match(comp_mesh_name, name_regex)); - if (is_match) { - ++changes; - SetObjectStencilID(mesh, object_id); - } -} bool UAirBlueprintLib::SetMeshStencilID(const std::string& mesh_name, int object_id, bool is_name_regex) { @@ -461,7 +337,7 @@ int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name) return -1; } -bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) +bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) { FCollisionQueryParams trace_params; trace_params.AddIgnoredActor(actor); @@ -471,8 +347,8 @@ bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, co return actor->GetWorld()->LineTraceTestByChannel(start, end, collision_channel, trace_params); } -bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, const FVector& end, - FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) +bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) { hit = FHitResult(ForceInit); @@ -484,8 +360,8 @@ bool UAirBlueprintLib::GetObstacle(const AActor* actor, const FVector& start, co return actor->GetWorld()->LineTraceSingleByChannel(hit, start, end, collision_channel, trace_params); } -bool UAirBlueprintLib::GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, - FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) +bool UAirBlueprintLib::GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, + FHitResult& hit, const AActor* ignore_actor, ECollisionChannel collision_channel) { TArray hits; @@ -536,47 +412,12 @@ void UAirBlueprintLib::FollowActor(AActor* follower, const AActor* followee, con follower->SetActorRotation(next_rot); } -template -FInputActionBinding& UAirBlueprintLib::BindActionToKey(const FName action_name, const FKey in_key, UserClass* actor, - typename FInputActionHandlerSignature::TUObjectMethodDelegate::FMethodPtr func, bool on_press_or_release, - bool shift_key, bool control_key, bool alt_key, bool command_key) -{ - FInputActionKeyMapping action(action_name, in_key, shift_key, control_key, alt_key, command_key); - - APlayerController* controller = actor->GetWorld()->GetFirstPlayerController(); - - controller->PlayerInput->AddActionMapping(action); - return controller->InputComponent-> - BindAction(action_name, on_press_or_release ? IE_Pressed : IE_Released, actor, func); -} - - -template -FInputAxisBinding& UAirBlueprintLib::BindAxisToKey(const FName axis_name, const FKey in_key, AActor* actor, UserClass* obj, - typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func) -{ - FInputAxisKeyMapping axis(axis_name, in_key); - - return UAirBlueprintLib::BindAxisToKey(axis, actor, obj, func); -} - -template -FInputAxisBinding& UAirBlueprintLib::BindAxisToKey(const FInputAxisKeyMapping& axis, AActor* actor, UserClass* obj, - typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func) -{ - APlayerController* controller = actor->GetWorld()->GetFirstPlayerController(); - - controller->PlayerInput->AddAxisMapping(axis); - return controller->InputComponent-> - BindAxis(axis.AxisName, obj, func); -} - int UAirBlueprintLib::RemoveAxisBinding(const FInputAxisKeyMapping& axis, FInputAxisBinding* axis_binding, AActor* actor) { if (axis_binding != nullptr && actor != nullptr) { APlayerController* controller = actor->GetWorld()->GetFirstPlayerController(); - + //remove mapping int found_mapping_index = -1, cur_mapping_index = -1; for (const auto& axis_arr : controller->PlayerInput->AxisMappings) { @@ -600,7 +441,7 @@ int UAirBlueprintLib::RemoveAxisBinding(const FInputAxisKeyMapping& axis, FInput } if (found_binding_index >= 0) controller->InputComponent->AxisBindings.RemoveAt(found_binding_index); - + return found_binding_index; } else return -1; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index ee799396b6..3b87091ba2 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -7,28 +7,31 @@ #include "GameFramework/Actor.h" #include "Components/InputComponent.h" #include "GameFramework/PlayerInput.h" -#include -#include +#include "IImageWrapperModule.h" #include "Kismet/BlueprintFunctionLibrary.h" #include "Kismet/KismetMathLibrary.h" #include "Components/MeshComponent.h" #include "LandscapeProxy.h" +#include "Kismet/GameplayStatics.h" + +#include "Runtime/Landscape/Classes/LandscapeComponent.h" #include "common/AirSimSettings.hpp" -#include "IImageWrapperModule.h" +#include +#include #include "AirBlueprintLib.generated.h" UENUM(BlueprintType) enum class LogDebugLevel : uint8 { - Informational UMETA(DisplayName="Informational"), + Informational UMETA(DisplayName = "Informational"), Success UMETA(DisplayName = "Success"), Failure UMETA(DisplayName = "Failure"), Unimportant UMETA(DisplayName = "Unimportant") }; /** - * - */ +* +*/ UCLASS() class UAirBlueprintLib : public UBlueprintFunctionLibrary { @@ -39,20 +42,40 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void OnEndPlay(); static void LogMessageString(const std::string &prefix, const std::string &suffix, LogDebugLevel level, float persist_sec = 60); UFUNCTION(BlueprintCallable, Category = "Utils") - static void LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec = 60); + static void LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec = 60); static float GetWorldToMetersScale(const AActor* context); template static T* GetActorComponent(AActor* actor, FString name); + template - static T* FindActor(const UObject* context, FString name); + static T* FindActor(const UObject* context, FString name) + { + TArray foundActors; + FindAllActor(context, foundActors); + FName name_n = FName(*name); + + for (AActor* actor : foundActors) { + if (actor->ActorHasTag(name_n) || actor->GetName().Compare(name) == 0) { + return static_cast(actor); + } + } + + //UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure); + return nullptr; + } + template - static void FindAllActor(const UObject* context, TArray& foundActors); - static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, + static void FindAllActor(const UObject* context, TArray& foundActors) + { + UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); + } + + static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); - static bool GetObstacle(const AActor* actor, const FVector& start, const FVector& end, + static bool GetObstacle(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); - static bool GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, + static bool GetLastObstaclePosition(const AActor* actor, const FVector& start, const FVector& end, FHitResult& hit, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); static void FollowActor(AActor* follower, const AActor* followee, const FVector& offset, bool fixed_z = false, float fixed_z_val = 2.0f); @@ -62,23 +85,63 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void InitializeMeshStencilIDs(bool ignore_existing); static bool IsInGameThread(); - + template - static std::string GetMeshName(T* mesh); + static std::string GetMeshName(T* mesh) + { + switch (mesh_naming_method_) + { + case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::OwnerName: + if (mesh->GetOwner()) + return std::string(TCHAR_TO_UTF8(*(mesh->GetOwner()->GetName()))); + else + return ""; // std::string(TCHAR_TO_UTF8(*(UKismetSystemLibrary::GetDisplayName(mesh)))); + case msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType::StaticMeshName: + if (mesh->GetStaticMesh()) + return std::string(TCHAR_TO_UTF8(*(mesh->GetStaticMesh()->GetName()))); + else + return ""; + default: + return ""; + } + } + static std::string GetMeshName(ALandscapeProxy* mesh); template static FInputActionBinding& BindActionToKey(const FName action_name, const FKey in_key, UserClass* actor, typename FInputActionHandlerSignature::TUObjectMethodDelegate< UserClass >::FMethodPtr func, bool on_press_or_release = false, - bool shift_key = false, bool control_key = false, bool alt_key = false, bool command_key = false); + bool shift_key = false, bool control_key = false, bool alt_key = false, bool command_key = false) + { + FInputActionKeyMapping action(action_name, in_key, shift_key, control_key, alt_key, command_key); + + APlayerController* controller = actor->GetWorld()->GetFirstPlayerController(); + + controller->PlayerInput->AddActionMapping(action); + return controller->InputComponent-> + BindAction(action_name, on_press_or_release ? IE_Pressed : IE_Released, actor, func); + } template static FInputAxisBinding& BindAxisToKey(const FName axis_name, const FKey in_key, AActor* actor, UserClass* obj, - typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func); + typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func) + { + FInputAxisKeyMapping axis(axis_name, in_key); + + return UAirBlueprintLib::BindAxisToKey(axis, actor, obj, func); + } + template static FInputAxisBinding& BindAxisToKey(const FInputAxisKeyMapping& axis, AActor* actor, UserClass* obj, - typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func); + typename FInputAxisHandlerSignature::TUObjectMethodDelegate::FMethodPtr func) + { + APlayerController* controller = actor->GetWorld()->GetFirstPlayerController(); + + controller->PlayerInput->AddAxisMapping(axis); + return controller->InputComponent-> + BindAxis(axis.AxisName, obj, func); + } static int RemoveAxisBinding(const FInputAxisKeyMapping& axis, FInputAxisBinding* axis_binding, AActor* actor); @@ -118,16 +181,87 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary private: template - static void InitializeObjectStencilID(T* obj, bool ignore_existing = true); + static void InitializeObjectStencilID(T* mesh, bool ignore_existing = true) + { + std::string mesh_name = common_utils::Utils::toLower(GetMeshName(mesh)); + if (mesh_name == "" || common_utils::Utils::startsWith(mesh_name, "default_")) { + //common_utils::Utils::DebugBreak(); + return; + } + FString name(mesh_name.c_str()); + int hash = 5; + for (int idx = 0; idx < name.Len(); ++idx) { + auto char_num = UKismetStringLibrary::GetCharacterAsNumber(name, idx); + if (char_num < 97) + continue; //numerics and other punctuations + hash += char_num; + } + if (ignore_existing || mesh->CustomDepthStencilValue == 0) { //if value is already set then don't bother + SetObjectStencilID(mesh, hash % 256); + } + } template - static void SetObjectStencilIDIfMatch(T* mesh, int object_id, - const std::string& mesh_name, bool is_name_regex, const std::regex& name_regex, int& changes); + static void SetObjectStencilIDIfMatch(T* mesh, int object_id, + const std::string& mesh_name, bool is_name_regex, const std::regex& name_regex, int& changes) + { + std::string comp_mesh_name = GetMeshName(mesh); + if (comp_mesh_name == "") + return; + bool is_match = (!is_name_regex && (comp_mesh_name == mesh_name)) + || (is_name_regex && std::regex_match(comp_mesh_name, name_regex)); + if (is_match) { + ++changes; + SetObjectStencilID(mesh, object_id); + } + } + template - static void SetObjectStencilID(T* mesh, int object_id); - static void SetObjectStencilID(ALandscapeProxy* mesh, int object_id); + static void SetObjectStencilID(T* mesh, int object_id) + { + if (object_id < 0) + { + mesh->SetRenderCustomDepth(false); + } + else + { + mesh->SetCustomDepthStencilValue(object_id); + mesh->SetRenderCustomDepth(true); + } + //mesh->SetVisibility(false); + //mesh->SetVisibility(true); + } + + static void SetObjectStencilID(ALandscapeProxy* mesh, int object_id) + { + if (object_id < 0) + { + mesh->bRenderCustomDepth = false; + } + else + { + mesh->CustomDepthStencilValue = object_id; + mesh->bRenderCustomDepth = true; + } + + // Explicitly set the custom depth state on the components so the + // render state is marked dirty and the update actually takes effect + // immediately. + for (ULandscapeComponent* comp : mesh->LandscapeComponents) + { + if (object_id < 0) + { + comp->SetRenderCustomDepth(false); + } + else + { + comp->SetCustomDepthStencilValue(object_id); + comp->SetRenderCustomDepth(true); + } + } + } static bool CompressUsingImageWrapper(const TArray& uncompressed, const int32 width, const int32 height, TArray& compressed); @@ -136,7 +270,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary //FViewPort doesn't expose this field so we are doing dirty work around by maintaining count by ourselves static uint32_t flush_on_draw_count_; static msr::airlib::AirSimSettings::SegmentationSettings::MeshNamingMethodType mesh_naming_method_; - + static IImageWrapperModule* image_wrapper_module_; }; diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp index bb4cb72f11..632ff677a6 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawn.cpp @@ -1,17 +1,19 @@ #include "CarPawn.h" -#include "CarWheelFront.h" -#include "CarWheelRear.h" -#include "common/common_utils/Utils.hpp" +#include "Engine/SkeletalMesh.h" +#include "GameFramework/Controller.h" #include "Components/TextRenderComponent.h" #include "Components/AudioComponent.h" #include "Sound/SoundCue.h" #include "WheeledVehicleMovementComponent4W.h" -#include "Engine/SkeletalMesh.h" -#include "GameFramework/Controller.h" + +#include "CarWheelFront.h" +#include "CarWheelRear.h" #include "AirBlueprintLib.h" -#include "common/ClockFactory.hpp" #include "PIPCamera.h" #include +#include "common/common_utils/Utils.hpp" +#include "common/ClockFactory.hpp" + #define LOCTEXT_NAMESPACE "VehiclePawn" @@ -208,7 +210,7 @@ void ACarPawn::initializeForBeginPlay(bool engine_sound) UAirBlueprintLib::LogMessageString("RC Controller on USB: ", joystick_state_.pid_vid == "" ? "(Detected)" : joystick_state_.pid_vid, LogDebugLevel::Informational); else - UAirBlueprintLib::LogMessageString("RC Controller on USB not detected: ", + UAirBlueprintLib::LogMessageString("RC Controller on USB not detected: ", std::to_string(joystick_state_.connection_error_code), LogDebugLevel::Informational); } @@ -412,7 +414,7 @@ void ACarPawn::updateCarControls() } //if API-client control is not active then we route keyboard/jostick control to car - if (! getApi()->isApiControlEnabled()) { + if (!getApi()->isApiControlEnabled()) { //all car controls from anywhere must be routed through API component getApi()->setCarControls(current_controls_); } @@ -439,9 +441,9 @@ void ACarPawn::updateForceFeedback() { // Update autocenter double speed = GetVehicleMovement()->GetForwardSpeed(); - joystick_.setAutoCenter(wrapper_->getRemoteControlID(), - ( 1.0 - 1.0 / ( std::abs(speed / 120) + 1.0)) - * (joystick_state_.left_x / 3)); + joystick_.setAutoCenter(wrapper_->getRemoteControlID(), + (1.0 - 1.0 / (std::abs(speed / 120) + 1.0)) + * (joystick_state_.left_x / 3)); } } diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h index 54f08d942f..51cb35c1a7 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.h @@ -1,10 +1,10 @@ #pragma once - #include "vehicles/car/api/CarApiBase.hpp" #include "VehiclePawnWrapper.h" #include "WheeledVehicleMovementComponent4W.h" #include "physics/Kinematics.hpp" +#include "common/ImageCaptureBase.hpp" class CarPawnApi : public msr::airlib::CarApiBase { diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 2f946af7e6..72e609f2c0 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -1,12 +1,14 @@ #include "SimModeCar.h" #include "ConstructorHelpers.h" + #include "AirBlueprintLib.h" #include "common/AirSimSettings.hpp" #include "AirBlueprintLib.h" -#include "Car/CarPawn.h" #ifndef AIRLIB_NO_RPC +#pragma warning(disable:4005) //warning C4005: 'TEXT': macro redefinition + #if defined _WIN32 || defined _WIN64 #include "AllowWindowsPlatformTypes.h" #endif @@ -86,8 +88,6 @@ void ASimModeCar::continueForTime(double seconds) void ASimModeCar::setupClockSpeed() { - ASimModeBase::setupClockSpeed(); //do the default setup, //TODO: may be we want explicit scalable clock? - current_clockspeed_ = getSettings().clock_speed; //setup clock in PhysX @@ -178,7 +178,7 @@ void ASimModeCar::setupVehiclesAndCamera(std::vector& vehicles) std::unique_ptr ASimModeCar::createApiServer() const { #ifdef AIRLIB_NO_RPC - return ASimMode::createApiServer(); + return ASimModeBase::createApiServer(); #else return std::unique_ptr(new msr::airlib::CarRpcLibServer( getSimModeApi(), getSettings().api_server_address)); @@ -240,7 +240,7 @@ void ASimModeCar::updateReport() { for (VehiclePtr vehicle : vehicles_) { VehiclePawnWrapper* wrapper = vehicle->getVehiclePawnWrapper(); - msr::airlib::StateReporter& reporter = * report_wrapper_.getReporter(); + msr::airlib::StateReporter& reporter = *report_wrapper_.getReporter(); std::string vehicle_name = fpv_vehicle_pawn_wrapper_->getVehicleConfigName(); reporter.writeHeading(std::string("Vehicle: ").append( diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index 2ce4c3ee9c..684c3cd8b4 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -1,6 +1,8 @@ #pragma once #include "CoreMinimal.h" + +#include "CarPawn.h" #include "common/Common.hpp" #include "SimMode/SimModeWorldBase.h" #include "VehiclePawnWrapper.h" diff --git a/Unreal/Plugins/AirSim/Source/ManualPoseController.h b/Unreal/Plugins/AirSim/Source/ManualPoseController.h index d2716eb3f9..bb6d978df2 100644 --- a/Unreal/Plugins/AirSim/Source/ManualPoseController.h +++ b/Unreal/Plugins/AirSim/Source/ManualPoseController.h @@ -2,6 +2,8 @@ #include "CoreMinimal.h" #include "GameFramework/Actor.h" +#include "GameFramework/PlayerInput.h" + #include "ManualPoseController.generated.h" UCLASS() diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp index 4913602535..b57cb5c79b 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.cpp @@ -1,16 +1,21 @@ #include "SimModeWorldMultiRotor.h" #include "ConstructorHelpers.h" +#include "Logging/MessageLog.h" +#include "Engine/World.h" +#include "GameFramework/PlayerController.h" + #include "AirBlueprintLib.h" #include "vehicles/multirotor/controllers/DroneControllerBase.hpp" #include "physics/PhysicsBody.hpp" #include "common/ClockFactory.hpp" #include -#include "Logging/MessageLog.h" #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" #include "UnrealSensors/UnrealSensorFactory.h" #ifndef AIRLIB_NO_RPC +#pragma warning(disable:4005) //warning C4005: 'TEXT': macro redefinition + #if defined _WIN32 || defined _WIN64 #include "AllowWindowsPlatformTypes.h" #endif @@ -39,7 +44,7 @@ void ASimModeWorldMultiRotor::BeginPlay() std::unique_ptr ASimModeWorldMultiRotor::createApiServer() const { #ifdef AIRLIB_NO_RPC - return ASimMode::createApiServer(); + return ASimModeBase::createApiServer(); #else return std::unique_ptr(new msr::airlib::MultirotorRpcLibServer( getSimModeApi(), getSettings().api_server_address)); @@ -102,7 +107,7 @@ void ASimModeWorldMultiRotor::setupVehiclesAndCamera(std::vector& ve } } - + //find all vehicle pawns { TArray pawns; @@ -198,7 +203,7 @@ void ASimModeWorldMultiRotor::createVehicles(std::vector& vehicles) ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawnWrapper* wrapper) { - std::shared_ptr sensor_factory = std::make_shared(wrapper->getPawn(), & wrapper->getNedTransform()); + std::shared_ptr sensor_factory = std::make_shared(wrapper->getPawn(), &wrapper->getNedTransform()); auto vehicle_params = MultiRotorParamsFactory::createConfig(wrapper->getVehicleConfigName(), sensor_factory); vehicle_params_.push_back(std::move(vehicle_params)); @@ -245,7 +250,7 @@ void ASimModeWorldMultiRotor::setupClockSpeed() else { //for slowing down, this don't generate instability ClockFactory::get(std::make_shared( - static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); } } else diff --git a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h index c49c8c2b72..f2b2e8e397 100644 --- a/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/Multirotor/SimModeWorldMultiRotor.h @@ -1,6 +1,8 @@ #pragma once #include "CoreMinimal.h" + +#include "FlyingPawn.h" #include "common/Common.hpp" #include "MultiRotorConnector.h" #include "vehicles/multirotor/MultiRotorParams.hpp" diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 4900aaa3a7..179881d71a 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -1,10 +1,11 @@ #pragma once #include "CoreMinimal.h" -#include "common/Common.hpp" #include "Kismet/KismetMathLibrary.h" #include "GameFramework/Actor.h" +#include "common/Common.hpp" + class NedTransform { diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 8b24a1d36e..5bfe8ee20e 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -3,6 +3,8 @@ #include "CoreMinimal.h" #include "Components/SceneCaptureComponent2D.h" #include "Camera/CameraActor.h" +#include "Materials/Material.h" + #include "common/ImageCaptureBase.hpp" #include "common/common_utils/Utils.hpp" #include "common/AirSimSettings.hpp" diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp index e5ac6b589c..bb57802a94 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.cpp @@ -1,8 +1,9 @@ #include "RecordingThread.h" +#include "TaskGraphInterfaces.h" #include "HAL/RunnableThread.h" + #include #include -#include "TaskGraphInterfaces.h" #include "RenderRequest.h" #include "PIPCamera.h" diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h index 983c605f06..7a381011b8 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingThread.h @@ -1,8 +1,9 @@ #pragma once #include "CoreMinimal.h" -#include "AirBlueprintLib.h" #include "HAL/Runnable.h" + +#include "AirBlueprintLib.h" #include "UnrealImageCapture.h" #include "VehiclePawnWrapper.h" #include "Recording/RecordingFile.h" diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp index 7379dc2ee6..0440d27135 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.cpp +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.cpp @@ -3,11 +3,12 @@ #include "Engine/TextureRenderTarget2D.h" #include "TaskGraphInterfaces.h" #include "ImageUtils.h" + #include "AirBlueprintLib.h" RenderRequest::RenderRequest(bool use_safe_method) - : use_safe_method_(use_safe_method), params_(nullptr), results_(nullptr), req_size_(0), - wait_signal_(new msr::airlib::WorkerThreadSignal) + : use_safe_method_(use_safe_method), params_(nullptr), results_(nullptr), req_size_(0), + wait_signal_(new msr::airlib::WorkerThreadSignal) { } @@ -24,7 +25,7 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v for (unsigned int i = 0; i < req_size; ++i) { results.push_back(std::make_shared()); - if (! params[i]->pixels_as_float) + if (!params[i]->pixels_as_float) results[i]->bmp.Reset(); else results[i]->bmp_float.Reset(); @@ -38,7 +39,7 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v for (unsigned int i = 0; i < req_size; ++i) { //TODO: below doesn't work right now because it must be running in game thread FIntPoint img_size; - if (! params[i]->pixels_as_float) { + if (!params[i]->pixels_as_float) { //below is documented method but more expensive because it forces flush FTextureRenderTargetResource* rt_resource = params[i]->render_target->GameThread_GetRenderTargetResource(); auto flags = setupRenderResource(rt_resource, params[i].get(), results[i].get(), img_size); @@ -62,13 +63,13 @@ void RenderRequest::getScreenshot(std::shared_ptr params[], std::v TGraphTask::CreateTask().ConstructAndDispatchWhenReady(*this); // wait for this task to complete - if (! wait_signal_->waitFor(5)) { + if (!wait_signal_->waitFor(5)) { throw std::runtime_error("timeout waiting for screenshot"); } } for (unsigned int i = 0; i < req_size; ++i) { - if (! params[i]->pixels_as_float) { + if (!params[i]->pixels_as_float) { if (results[i]->width != 0 && results[i]->height != 0) { results[i]->image_data_uint8.SetNumUninitialized(results[i]->width * results[i]->height * 4, false); if (params[i]->compress) @@ -119,7 +120,7 @@ void RenderRequest::ExecuteTask() //should we be using ENQUEUE_UNIQUE_RENDER_COMMAND_ONEPARAMETER which was in original commit by @saihv //https://github.com/Microsoft/AirSim/pull/162/commits/63e80c43812300a8570b04ed42714a3f6949e63f#diff-56b790f9394f7ca1949ddbb320d8456fR64 - if (! params_[i]->pixels_as_float) { + if (!params_[i]->pixels_as_float) { //below is undocumented method that avoids flushing, but it seems to segfault every 2000 or so calls RHICmdList.ReadSurfaceData( rhi_texture, diff --git a/Unreal/Plugins/AirSim/Source/RenderRequest.h b/Unreal/Plugins/AirSim/Source/RenderRequest.h index 84eb14dc69..7503a9bed0 100644 --- a/Unreal/Plugins/AirSim/Source/RenderRequest.h +++ b/Unreal/Plugins/AirSim/Source/RenderRequest.h @@ -1,10 +1,13 @@ #pragma once #include "CoreMinimal.h" +#include "Engine/TextureRenderTarget2D.h" + #include #include "common/common_utils/WorkerThread.hpp" #include "common/Common.hpp" + class RenderRequest : public FRenderCommand { public: diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index e82def7fbe..a6b24cd15a 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -1,12 +1,15 @@ #include "SimHUD.h" #include "ConstructorHelpers.h" +#include "Kismet/KismetSystemLibrary.h" +#include "Misc/FileHelper.h" + #include "Multirotor/SimModeWorldMultiRotor.h" #include "Car/SimModeCar.h" #include "common/AirSimSettings.hpp" -#include "Kismet/KismetSystemLibrary.h" - +#include "api/DebugApiServer.hpp" #include + ASimHUD* ASimHUD::instance_ = nullptr; ASimHUD::ASimHUD() diff --git a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp index 575ef16dc0..1890822668 100644 --- a/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp +++ b/Unreal/Plugins/AirSim/Source/SimJoyStick/DirectInputJoyStick.cpp @@ -1,4 +1,5 @@ #include "DirectInputJoystick.h" +#include "UnrealMathUtility.h" #if defined _WIN32 || defined _WIN64 @@ -6,15 +7,23 @@ #define DIRECTINPUT_VERSION 0x0800 #endif +//remove warnings for using safe VC++ APIs +#define _CRT_SECURE_NO_WARNINGS 1 +#pragma warning(disable:4996) //warning C4996: This function or variable may be unsafe. Consider using xxx instead. +#pragma warning(disable:4005) //warning C4005: 'TEXT': macro redefinition + +#include "AllowWindowsPlatformTypes.h" #include "common/common_utils/MinWinDefines.hpp" #include +#include "HideWindowsPlatformTypes.h" + #pragma warning(push) #pragma warning(disable:6000 28251) #include #pragma warning(pop) #include -#include "UnrealMathUtility.h" +#include //SysAllocString // Stuff to filter out XInput devices #ifndef FALSE @@ -33,7 +42,7 @@ #define DIJT_SAFE_DELETE(p) { if(p) { delete (p); (p)=nullptr; } } #define DIJT_SAFE_RELEASE(p) { if(p) { (p)->Release(); (p)=nullptr; } } -struct DirectInputJoyStick::impl{ +struct DirectInputJoyStick::impl { private: struct XINPUT_DEVICE_NODE { @@ -73,15 +82,15 @@ struct DirectInputJoyStick::impl{ } // Magnitude ranges from -1 to 1 - void setAutoCenterStrength(double magnitude) + void setAutoCenterStrength(double magnitude) { DICONSTANTFORCE cf = { magnitude * 10000 }; - + g_sAutoCenterConfig.cbTypeSpecificParams = sizeof(DICONSTANTFORCE); g_sAutoCenterConfig.lpvTypeSpecificParams = &cf; if (g_pAutoCenterHandle) { - g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | + g_pAutoCenterHandle->SetParameters(&g_sAutoCenterConfig, DIEP_DIRECTION | DIEP_TYPESPECIFICPARAMS | DIEP_START); } @@ -90,11 +99,11 @@ struct DirectInputJoyStick::impl{ #define FFWRMAX 0.08 // Strength ranges from 0 to 1 - void setWheelRumbleStrength(double strength) + void setWheelRumbleStrength(double strength) { DIPERIODIC pf = { FFWRMAX * strength * 10000,0,0,0.06 * 1000000 }; - - g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); + + g_sWheelRumbleConfig.cbTypeSpecificParams = sizeof(DIPERIODIC); g_sWheelRumbleConfig.lpvTypeSpecificParams = &pf; if (g_pWheelRumbleHandle) { @@ -127,7 +136,7 @@ struct DirectInputJoyStick::impl{ } private: - std::string toString(const std::wstring& wstr) + std::string toString(const std::wstring& wstr) { return std::string(wstr.begin(), wstr.end()); } @@ -143,7 +152,7 @@ struct DirectInputJoyStick::impl{ return g; } - HRESULT InitForceFeedback() + HRESULT InitForceFeedback() { HRESULT hr; @@ -477,7 +486,7 @@ struct DirectInputJoyStick::impl{ { DirectInputJoyStick::impl *obj = reinterpret_cast(pContext); - auto pEnumContext = & obj->enumContext; + auto pEnumContext = &obj->enumContext; HRESULT hr; if (obj->g_bFilterOutXinputDevices && obj->IsXInputDevice(&pdidInstance->guidProduct)) @@ -609,7 +618,7 @@ struct DirectInputJoyStick::impl{ DIJOYSTATE2 js; // DInput joystick state state.is_valid = false; - + if (!g_pJoystick) { state.message = "No device at index"; return S_OK; @@ -620,7 +629,7 @@ struct DirectInputJoyStick::impl{ if (FAILED(hr)) { state.message = "device stream interrupted"; - + // DInput is telling us that the input stream has been // interrupted. We aren't tracking any state between polls, so // we don't have any special reset that needs to be done. We diff --git a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp index 1bfd2981bc..3bb14973de 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealImageCapture.cpp @@ -1,6 +1,8 @@ #include "UnrealImageCapture.h" -#include "RenderRequest.h" +#include "Engine/World.h" #include "ImageUtils.h" + +#include "RenderRequest.h" #include "common/ClockFactory.hpp" diff --git a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h index 554ac3796c..7d7cff8d2d 100644 --- a/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h +++ b/Unreal/Plugins/AirSim/Source/VehiclePawnWrapper.h @@ -1,6 +1,8 @@ #pragma once #include "CoreMinimal.h" +#include "GameFramework/Pawn.h" + #include #include #include "UnrealImageCapture.h" @@ -10,7 +12,7 @@ #include "physics/Kinematics.hpp" #include "NedTransform.h" #include "api/VehicleApiBase.hpp" -#include "GameFramework/Pawn.h" + class VehiclePawnWrapper { diff --git a/clean.cmd b/clean.cmd index 0a40ceb61e..62ccaef0eb 100644 --- a/clean.cmd +++ b/clean.cmd @@ -2,6 +2,7 @@ rd /s/q AirLib\lib rd /s/q AirLib\deps\MavLinkCom rd /s/q AirLib\deps\rpclib rd /s/q external\rpclib\build +rd /s/q external msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln /t:Clean if ERRORLEVEL 1 goto :buildfailed msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln /t:Clean From 2bcecb35f6d14233fc1538bffe111b7b035affc5 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 30 Apr 2018 14:02:05 -0700 Subject: [PATCH 120/121] remove rd external from clean.cmd --- clean.cmd | 2 +- clean_rebuild.bat | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/clean.cmd b/clean.cmd index 62ccaef0eb..de777111be 100644 --- a/clean.cmd +++ b/clean.cmd @@ -2,7 +2,7 @@ rd /s/q AirLib\lib rd /s/q AirLib\deps\MavLinkCom rd /s/q AirLib\deps\rpclib rd /s/q external\rpclib\build -rd /s/q external + msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln /t:Clean if ERRORLEVEL 1 goto :buildfailed msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln /t:Clean diff --git a/clean_rebuild.bat b/clean_rebuild.bat index 4af6112702..732ecbfa26 100644 --- a/clean_rebuild.bat +++ b/clean_rebuild.bat @@ -3,6 +3,7 @@ REM //---------- set up variable ---------- setlocal set ROOT_DIR=%~dp0 +rd /s/q external git clean -ffdx git pull build.cmd From 318dcf9c19dd05934f73e57160e2caab56f0fd81 Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 30 Apr 2018 14:02:58 -0700 Subject: [PATCH 121/121] fix Linux compile errors --- Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp | 1 - Unreal/Plugins/AirSim/Source/AirBlueprintLib.h | 1 + Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 60794c488e..c0d46024fd 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -12,7 +12,6 @@ #include "UObjectIterator.h" #include "Camera/CameraComponent.h" //#include "Runtime/Foliage/Public/FoliageType.h" -#include "Kismet/KismetStringLibrary.h" #include "MessageDialog.h" #include "Engine/LocalPlayer.h" #include "Engine/SkeletalMesh.h" diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 3b87091ba2..c0e0a8c51b 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -13,6 +13,7 @@ #include "Components/MeshComponent.h" #include "LandscapeProxy.h" #include "Kismet/GameplayStatics.h" +#include "Kismet/KismetStringLibrary.h" #include "Runtime/Landscape/Classes/LandscapeComponent.h" #include "common/AirSimSettings.hpp" diff --git a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp index e104d6caf9..a0d9b27ed4 100644 --- a/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/CarPawnApi.cpp @@ -126,7 +126,7 @@ void CarPawnApi::reset() pawn_->reset(); for (auto* phys_comp : phys_comps) { - phys_comp->SetPhysicsAngularVelocity(FVector::ZeroVector); + phys_comp->SetPhysicsAngularVelocityInDegrees(FVector::ZeroVector); phys_comp->SetPhysicsLinearVelocity(FVector::ZeroVector); phys_comp->SetSimulatePhysics(false); }