diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 6484578985..5e3a0845ae 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -50,7 +50,10 @@ class RpcLibClientBase { vector simListSceneObjects(const string& name_regex = string(".*")) const; Pose simGetObjectPose(const std::string& object_name) const; + bool simLoadLevel(const string& level_name); + Vector3r simGetObjectScale(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); + bool simSetObjectScale(const std::string& object_name, const Vector3r& scale); //task management APIs void cancelLastTask(const std::string& vehicle_name = ""); diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 799c2eb09f..e07cc96363 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -25,6 +25,11 @@ class WorldSimApiBase { virtual ~WorldSimApiBase() = default; + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string& level_name) = 0; + virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale) = 0; + virtual bool destroyObject(const string& object_name) = 0; + virtual bool isPaused() const = 0; virtual void reset() = 0; virtual void pause(bool is_paused) = 0; @@ -54,7 +59,9 @@ class WorldSimApiBase { virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; + virtual Vector3r getObjectScale(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0; virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0; virtual vector getMeshPositionVertexBuffers() const = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index ce9fd75f5d..4e91492e51 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -25,8 +25,8 @@ struct AirSimSettings { public: //types static constexpr int kSubwindowCount = 3; //must be >= 3 for now static constexpr char const * kVehicleTypePX4 = "px4multirotor"; - static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; - static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; + static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; + static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; static constexpr char const * kVehicleTypeArduCopter = "arducopter"; static constexpr char const * kVehicleTypePhysXCar = "physxcar"; static constexpr char const * kVehicleTypeArduRover = "ardurover"; @@ -294,9 +294,9 @@ struct AirSimSettings { std::map params; }; - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; struct SegmentationSetting { enum class InitMethodType { @@ -327,6 +327,7 @@ struct AirSimSettings { public: //fields std::string simmode_name = ""; + std::string level_name = ""; std::vector subwindow_settings; RecordingSetting recording_setting; @@ -340,7 +341,7 @@ struct AirSimSettings { int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME bool enable_rpc = true; std::string api_server_address = ""; - int api_port = RpcLibPort; + int api_port = RpcLibPort; std::string physics_engine_name = ""; std::string clock_type = ""; @@ -352,8 +353,8 @@ struct AirSimSettings { std::map> vehicles; CameraSetting camera_defaults; CameraDirectorSetting camera_director; - float speed_unit_factor = 1.0f; - std::string speed_unit_label = "m\\s"; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; public: //methods @@ -379,6 +380,7 @@ struct AirSimSettings { checkSettingsVersion(settings_json); loadCoreSimModeSettings(settings_json, simmode_getter); + loadLevelSettings(settings_json); loadDefaultCameraSetting(settings_json, camera_defaults); loadCameraDirectorSetting(settings_json, camera_director, simmode_name); loadSubWindowsSettings(settings_json, subwindow_settings); @@ -514,6 +516,11 @@ struct AirSimSettings { physics_engine_name = "PhysX"; //this value is only informational for now } } + + void loadLevelSettings(const Settings& settings_json) + { + level_name = settings_json.getString("Default Environment", ""); + } void loadViewModeSettings(const Settings& settings_json) { @@ -626,7 +633,7 @@ struct AirSimSettings { { //these settings_json are expected in same section, not in another child std::unique_ptr vehicle_setting_p = std::unique_ptr(new MavLinkVehicleSetting()); - MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); + MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); //TODO: we should be selecting remote if available else keyboard //currently keyboard is not supported so use rc as default @@ -1021,7 +1028,7 @@ struct AirSimSettings { //because for docker container default is 0.0.0.0 and people get really confused why things //don't work api_server_address = settings_json.getString("LocalHostIp", ""); - api_port = settings_json.getInt("ApiServerPort", RpcLibPort); + api_port = settings_json.getInt("ApiServerPort", RpcLibPort); is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); engine_sound = settings_json.getBool("EngineSound", false); enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index b76a5a2f00..89b7a52982 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -351,23 +351,41 @@ std::vector RpcLibClientBase::simSwapTextures(const std::string& ta return pimpl_->client.call("simSwapTextures", tags, tex_id, component_id, material_id).as>(); } +bool RpcLibClientBase::simLoadLevel(const string& level_name) +{ + return pimpl_->client.call("simLoadLevel", level_name).as(); +} + +msr::airlib::Vector3r RpcLibClientBase::simGetObjectScale(const std::string& object_name) const +{ + return pimpl_->client.call("simGetObjectScale", object_name).as().to(); +} + msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); } + bool RpcLibClientBase::simSetObjectPose(const std::string& object_name, const msr::airlib::Pose& pose, bool teleport) { return pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose), teleport).as(); } +bool RpcLibClientBase::simSetObjectScale(const std::string& object_name, const msr::airlib::Vector3r& scale) +{ + return pimpl_->client.call("simSetObjectScale", object_name, RpcLibAdapatorsBase::Vector3r(scale)).as(); +} + CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const { return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name).as().to(); } + void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name) { pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdapatorsBase::Pose(pose), vehicle_name); } + void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name) { pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 8ff8a90a82..43e62e5341 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -83,9 +83,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_.reset(new impl(server_address, port)); pimpl_->server.bind("ping", [&]() -> bool { return true; }); + pimpl_->server.bind("getServerVersion", []() -> int { return 1; }); + pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { return 1; }); @@ -93,9 +95,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simPause", [&](bool is_paused) -> void { getWorldSimApi()->pause(is_paused); }); + pimpl_->server.bind("simIsPaused", [&]() -> bool { return getWorldSimApi()->isPaused(); }); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { getWorldSimApi()->continueForTime(seconds); }); @@ -109,6 +113,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simEnableWeather", [&](bool enable) -> void { getWorldSimApi()->enableWeather(enable); }); + pimpl_->server.bind("simSetWeatherParameter", [&](WorldSimApiBase::WeatherParameter param, float val) -> void { getWorldSimApi()->setWeatherParameter(param, val); }); @@ -116,17 +121,21 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->enableApiControl(is_enabled); }); + pimpl_->server.bind("isApiControlEnabled", [&](const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->isApiControlEnabled(); }); + pimpl_->server.bind("armDisarm", [&](bool arm, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->armDisarm(arm); }); + pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> vector { const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); return RpcLibAdapatorsBase::ImageResponse::from(response); }); + pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { auto result = getVehicleSimApi(vehicle_name)->getImage(camera_name, type); if (result.size() == 0) { @@ -145,6 +154,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision); }); + pimpl_->server.bind("simGetVehiclePose", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getVehicleSimApi(vehicle_name)->getPose(); return RpcLibAdapatorsBase::Pose(pose); @@ -248,32 +258,58 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return getWorldSimApi()->listSceneObjects(name_regex); }); + pimpl_->server.bind("simLoadLevel", [&](const std::string& level_name) -> bool { + return getWorldSimApi()->loadLevel(level_name); + }); + + pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdapatorsBase::Pose& pose, const RpcLibAdapatorsBase::Vector3r& scale) -> string { + return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to()); + }); + + pimpl_->server.bind("simDestroyObject", [&](const string& object_name) -> bool { + return getWorldSimApi()->destroyObject(object_name); + }); + pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); + + pimpl_->server.bind("simGetObjectScale", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Vector3r { + const auto& scale = getWorldSimApi()->getObjectScale(object_name); + return RpcLibAdapatorsBase::Vector3r(scale); + }); + pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose, bool teleport) -> bool { return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport); }); + pimpl_->server.bind("simSetObjectScale", [&](const std::string& object_name, const RpcLibAdapatorsBase::Vector3r& scale) -> bool { + return getWorldSimApi()->setObjectScale(object_name, scale.to()); + }); + pimpl_->server.bind("simFlushPersistentMarkers", [&]() -> void { getWorldSimApi()->simFlushPersistentMarkers(); }); + pimpl_->server.bind("simPlotPoints", [&](const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent); }); + pimpl_->server.bind("simPlotLineStrip", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotLineList", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotArrows", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { vector conv_points_start; RpcLibAdapatorsBase::to(points_start, conv_points_start); @@ -281,21 +317,25 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& RpcLibAdapatorsBase::to(points_end, conv_points_end); getWorldSimApi()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); }); + pimpl_->server.bind("simPlotStrings", [&](const std::vector strings, const std::vector& positions, float scale, const vector& color_rgba, float duration) -> void { vector conv_positions; RpcLibAdapatorsBase::to(positions, conv_positions); getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration); }); + pimpl_->server.bind("simPlotTransforms", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { vector conv_poses; RpcLibAdapatorsBase::to(poses, conv_poses); getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotTransformsWithNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) -> void { vector conv_poses; RpcLibAdapatorsBase::to(poses, conv_poses); getWorldSimApi()->simPlotTransformsWithNames(conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); }); + pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); return RpcLibAdapatorsBase::KinematicsState(result); @@ -310,9 +350,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getVehicleApi(vehicle_name)->cancelLastTask(); }); - pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { - return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); - }); + pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { + return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); + }); pimpl_->server.bind("startRecording", [&]() -> void { getWorldSimApi()->startRecording(); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 94a00cfd08..d352439ca0 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -74,6 +74,10 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con return UnityUtilities::Convert_to_Pose(airSimPose); } +msr::airlib::Vector3r WorldSimApi::getObjectScale(const std::string& object_name) const { return Vector3r(); } +msr::airlib::Vector3r WorldSimApi::getObjectScaleInternal(const std::string& object_name) const { return Vector3r(); } +bool WorldSimApi::setObjectScale(const std::string& object_name, const Vector3r& scale) { return false; } + bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimApi::Pose& pose, bool teleport) { AirSimUnity::AirSimPose airSimPose = UnityUtilities::Convert_to_AirSimPose(pose); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index a69f48bdff..a947d7cf6b 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -13,6 +13,12 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase WorldSimApi(SimModeBase* simmode, std::string vehicle_name); virtual ~WorldSimApi(); + + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string& level_name) { return false; }; + virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale) { return ""; }; + virtual bool destroyObject(const std::string& object_name) { return false; }; + virtual bool isPaused() const override; virtual void reset() override; virtual void pause(bool is_paused) override; @@ -31,7 +37,11 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; + + virtual Vector3r getObjectScale(const std::string& object_name) const override; + Vector3r getObjectScaleInternal(const std::string& object_name) const; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) override; //----------- Plotting APIs ----------/ virtual void simFlushPersistentMarkers() override; diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset new file mode 100644 index 0000000000..256ffb5bc1 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset new file mode 100644 index 0000000000..e2469f3564 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG new file mode 100644 index 0000000000..7cd3e4f6e9 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset new file mode 100644 index 0000000000..131e0077ff Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset new file mode 100644 index 0000000000..ed11301a3b Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset differ diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index e1f62ce1f3..653704fc41 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -9,9 +9,10 @@ #include "Components/StaticMeshComponent.h" #include "EngineUtils.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" -#include "UObject/UObjectIterator.h" +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" +#include "UObject/UObjectIterator.h" #include "Camera/CameraComponent.h" -//#include "Runtime/Foliage/Public/FoliageType.h" +#include "Runtime/Engine/Classes/GameFramework/PlayerStart.h" #include "Misc/MessageDialog.h" #include "Engine/LocalPlayer.h" #include "Engine/SkeletalMesh.h" @@ -19,6 +20,7 @@ #include "IImageWrapper.h" #include "Misc/ObjectThumbnail.h" #include "Engine/Engine.h" +#include "Engine/World.h" #include #include "common/common_utils/Utils.hpp" #include "Modules/ModuleManager.h" @@ -30,6 +32,7 @@ Methods -> CamelCase parameters -> camel_case */ +ULevelStreamingDynamic *UAirBlueprintLib::CURRENT_LEVEL = nullptr; bool UAirBlueprintLib::log_messages_hidden_ = false; msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method_ = msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType::OwnerName; @@ -73,6 +76,45 @@ void UAirBlueprintLib::setSimulatePhysics(AActor* actor, bool simulate_physics) } } +ULevelStreamingDynamic* UAirBlueprintLib::loadLevel(UObject* context, const FString& level_name) +{ + bool success{ false }; + context->GetWorld()->SetNewWorldOrigin(FIntVector(0,0,0)); + ULevelStreamingDynamic* new_level = UAirsimLevelStreaming::LoadAirsimLevelInstance( + context->GetWorld(), level_name, FVector(0, 0, 0), FRotator(0, 0, 0), success); + if (success) + { + if(CURRENT_LEVEL != nullptr && CURRENT_LEVEL->IsValidLowLevel()) + CURRENT_LEVEL->SetShouldBeLoaded(false); + CURRENT_LEVEL = new_level; + } + return CURRENT_LEVEL; +} + +bool UAirBlueprintLib::spawnPlayer(UWorld* context) +{ + + bool success{ false }; + TArray player_start_actors; + FindAllActor(context, player_start_actors); + if (player_start_actors.Num() > 1) + { + for (auto player_start : player_start_actors) + { + if (player_start->GetName() != FString("SuperStart")) + { + //context->GetWorld()->SetNewWorldOrigin(FIntVector(0, 0, 0)); + auto location = player_start->GetActorLocation(); + context->RequestNewWorldOrigin(FIntVector(location.X, location.Y, location.Z)); + success = true; + break; + } + } + } + return success; +} + + std::vector UAirBlueprintLib::getPhysicsComponents(AActor* actor) { std::vector phys_comps; @@ -474,6 +516,46 @@ std::vector UAirBlueprintLib::Ge return meshes; } + +TArray UAirBlueprintLib::ListWorldsInRegistry() +{ + FARFilter Filter; + Filter.ClassNames.Add(UWorld::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + TArray WorldNames; + for (auto asset : AssetData) + WorldNames.Add(asset.AssetName); + return WorldNames; +} + +UObject* UAirBlueprintLib::GetMeshFromRegistry(const std::string& load_object) +{ + FARFilter Filter; + Filter.ClassNames.Add(UStaticMesh::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + UObject* LoadObject = NULL; + for (auto asset : AssetData) + { + UE_LOG(LogTemp, Log, TEXT("Asset path: %s"), *asset.PackagePath.ToString()); + if (asset.AssetName == FName(load_object.c_str())) + { + LoadObject = asset.GetAsset(); + break; + } + } + return LoadObject; +} + bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) { FCollisionQueryParams trace_params; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 17f01b9201..bddcff4dbb 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -4,8 +4,10 @@ #pragma once #include "CoreMinimal.h" +#include "Runtime/AssetRegistry/Public/AssetRegistryModule.h" #include "GameFramework/Actor.h" #include "Components/InputComponent.h" +#include "EngineUtils.h" #include "GameFramework/PlayerInput.h" #include "IImageWrapperModule.h" #include "Kismet/BlueprintFunctionLibrary.h" @@ -15,13 +17,16 @@ #include "Kismet/GameplayStatics.h" #include "Kismet/KismetStringLibrary.h" #include "Engine/World.h" - #include "Runtime/Landscape/Classes/LandscapeComponent.h" +#include "Runtime/Engine/Classes/Kismet/GameplayStatics.h" +#include "AirsimLevelStreaming.h" +#include "Runtime/Core/Public/HAL/FileManager.h" #include "common/AirSimSettings.hpp" #include #include #include "AirBlueprintLib.generated.h" +class ULevelStreamingDynamic; UENUM(BlueprintType) enum class LogDebugLevel : uint8 { @@ -46,25 +51,21 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UFUNCTION(BlueprintCallable, Category = "Utils") 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) { - 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); + for (TActorIterator It(context->GetWorld(), T::StaticClass()); It; ++It) + { + AActor* Actor = *It; + if (!Actor->IsPendingKill() && (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; } @@ -73,8 +74,17 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary { UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } + + static ULevelStreamingDynamic *CURRENT_LEVEL; static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static ULevelStreamingDynamic* loadLevel(UObject* context, const FString& level_name); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static bool spawnPlayer(UWorld* context); + UFUNCTION(BlueprintPure, Category = "AirSim|LevelAPI") + static TArray ListWorldsInRegistry(); + static UObject* GetMeshFromRegistry(const std::string& load_object); static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); @@ -182,9 +192,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void setUnrealClockSpeed(const AActor* context, float clock_speed); static IImageWrapperModule* getImageWrapperModule(); static void CompressImageArray(int32 width, int32 height, const TArray &src, TArray &dest); - - static std::vector GetStaticMeshComponents(); - + static std::vector GetStaticMeshComponents(); private: template static void InitializeObjectStencilID(T* mesh, bool ignore_existing = true) diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 6ba94e59c0..11970b262a 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -78,7 +78,7 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry","PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore" }); //suppress VC++ proprietary warnings diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp new file mode 100644 index 0000000000..29f634c510 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp @@ -0,0 +1,51 @@ +#pragma once + +#include "AirsimLevelStreaming.h" +#include "Engine/World.h" + +int32 UAirsimLevelStreaming::LevelInstanceId = 0; + + +UAirsimLevelStreaming* UAirsimLevelStreaming::LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess) +{ + if (!WorldContextObject) + { + return nullptr; + } + + // Check whether requested map exists, this could be very slow if LevelName is a short package name + FString LongPackageName; + bool success = FPackageName::SearchForPackageOnDisk(LevelName, &LongPackageName); + if (!success) +{ + bOutSuccess = false; + return nullptr; + } + + // Create Unique Name for sub-level package + const FString ShortPackageName = FPackageName::GetShortName(LongPackageName); + const FString PackagePath = FPackageName::GetLongPackagePath(LongPackageName); + FString UniqueLevelPackageName = PackagePath + TEXT("/") + WorldContextObject->StreamingLevelsPrefix + ShortPackageName; + UniqueLevelPackageName += TEXT("_LevelInstance_") + FString::FromInt(++LevelInstanceId); + + // Setup streaming level object that will load specified map + ULevelStreamingDynamic* level_pointer = NewObject(WorldContextObject, ULevelStreamingDynamic::StaticClass(), NAME_None, RF_Transient, NULL); + level_pointer->SetWorldAssetByPackageName(FName(*UniqueLevelPackageName)); + level_pointer->LevelColor = FColor::MakeRandomColor(); + level_pointer->SetShouldBeLoaded(true); + level_pointer->SetShouldBeVisible(true); + level_pointer->bShouldBlockOnLoad = true; + level_pointer->bInitiallyLoaded = true; + level_pointer->bInitiallyVisible = true; + + // Transform + level_pointer->LevelTransform = FTransform(Rotation, Location); + // Map to Load + level_pointer->PackageNameToLoad = FName(*LongPackageName); + // Add the new level to world. + WorldContextObject->AddStreamingLevel(level_pointer); + + bOutSuccess = true; + + return dynamic_cast (level_pointer); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h new file mode 100644 index 0000000000..d45570127e --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h @@ -0,0 +1,12 @@ +#pragma once +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" + + +class UAirsimLevelStreaming : public ULevelStreamingDynamic +{ +public: + static UAirsimLevelStreaming* LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess); + +private: + static int32 LevelInstanceId; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 77a56954d5..be48b7150c 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -23,6 +23,8 @@ void ASimHUD::BeginPlay() try { UAirBlueprintLib::OnBeginPlay(); + loadLevel(); + initializeSettings(); setUnrealEngineSettings(); createSimMode(); @@ -282,6 +284,13 @@ std::string ASimHUD::getSimModeFromUser() return "Car"; } +void ASimHUD::loadLevel() +{ + if (AirSimSettings::singleton().level_name != "") + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str()));}, true); + else + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString("Blocks"));}, true); +} void ASimHUD::createSimMode() { std::string simmode_name = AirSimSettings::singleton().simmode_name; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h index 06bde50870..fab7fdf82f 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h @@ -59,6 +59,7 @@ class AIRSIM_API ASimHUD : public AHUD void createSimMode(); void initializeSettings(); void setUnrealEngineSettings(); + void loadLevel(); void createMainWidget(); const std::vector& getSubWindowSettings() const; std::vector& getSubWindowSettings(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h new file mode 100644 index 0000000000..93bd97e793 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h @@ -0,0 +1,15 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "CoreMinimal.h" +#include "Blueprint/UserWidget.h" +#include "Runtime/UMG/Public/Components/Image.h" +#include "LoadingScreenWidget.generated.h" + + +UCLASS() +class AIRSIM_API ULoadingScreenWidget : public UUserWidget +{ + GENERATED_BODY() +}; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 1aa75cc494..e86f100223 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -26,9 +26,17 @@ //it to AirLib and directly implement WorldSimApiBase interface #include "WorldSimApi.h" +ASimModeBase *ASimModeBase::SIMMODE = nullptr; + +ASimModeBase* ASimModeBase::getSimMode() +{ + return SIMMODE; +} ASimModeBase::ASimModeBase() { + SIMMODE = this; + static ConstructorHelpers::FClassFinder external_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); external_camera_class_ = external_camera_class.Succeeded() ? external_camera_class.Class : nullptr; static ConstructorHelpers::FClassFinder camera_director_class(TEXT("Blueprint'/AirSim/Blueprints/BP_CameraDirector'")); @@ -47,6 +55,30 @@ ASimModeBase::ASimModeBase() static ConstructorHelpers::FClassFinder sky_sphere_class(TEXT("Blueprint'/Engine/EngineSky/BP_Sky_Sphere'")); sky_sphere_class_ = sky_sphere_class.Succeeded() ? sky_sphere_class.Class : nullptr; + + static ConstructorHelpers::FClassFinder loading_screen_class_find(TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_LoadingScreenWidget'")); + if (loading_screen_class_find.Succeeded()) + { + auto loading_screen_class = loading_screen_class_find.Class; + loading_screen_widget_ = CreateWidget(this->GetWorld(), loading_screen_class); + + } + else + loading_screen_widget_ = nullptr; + +} + +void ASimModeBase::toggleLoadingScreen(bool is_visible) +{ + if (loading_screen_widget_ == nullptr) + return; + else { + + if (is_visible) + loading_screen_widget_->SetVisibility(ESlateVisibility::Visible); + else + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); + } } void ASimModeBase::BeginPlay() @@ -90,6 +122,9 @@ void ASimModeBase::BeginPlay() UWeatherLib::initWeather(World, spawned_actors_); //UWeatherLib::showWeatherMenu(World); } + + loading_screen_widget_->AddToViewport(); + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); } const NedTransform& ASimModeBase::getGlobalNedTransform() @@ -110,7 +145,6 @@ void ASimModeBase::checkVehicleReady() UAirBlueprintLib::LogMessage("Tip: check connection info in settings.json", "", LogDebugLevel::Informational); } } - } } @@ -119,8 +153,7 @@ void ASimModeBase::setStencilIDs() UAirBlueprintLib::SetMeshNamingMethod(getSettings().segmentation_setting.mesh_naming_method); if (getSettings().segmentation_setting.init_method == - AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { - + AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { UAirBlueprintLib::InitializeMeshStencilIDs(!getSettings().segmentation_setting.override_existing); } //else don't init @@ -637,7 +670,6 @@ void ASimModeBase::drawLidarDebugPoints() msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); if (api != nullptr) { - msr::airlib::uint count_lidars = api->getSensors().size(msr::airlib::SensorBase::SensorType::Lidar); for (msr::airlib::uint i = 0; i < count_lidars; i++) { diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index bf116989dd..b357a6c1fb 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -14,9 +14,11 @@ #include "api/ApiProvider.hpp" #include "PawnSimApi.h" #include "common/StateReporterWrapper.hpp" +#include "LoadingScreenWidget.h" #include "SimModeBase.generated.h" +DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLevelLoaded); UCLASS() class AIRSIM_API ASimModeBase : public AActor @@ -25,6 +27,9 @@ class AIRSIM_API ASimModeBase : public AActor GENERATED_BODY() + UPROPERTY(BlueprintAssignable, BlueprintCallable) + FLevelLoaded OnLevelLoaded; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Refs") ACameraDirector* CameraDirector; @@ -34,7 +39,15 @@ class AIRSIM_API ASimModeBase : public AActor UFUNCTION(BlueprintCallable, Category = "Recording") bool toggleRecording(); -public: + UFUNCTION(BlueprintPure, Category = "Airsim | get stuff") + static ASimModeBase* getSimMode(); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + void toggleLoadingScreen(bool is_visible); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + virtual void reset(); + // Sets default values for this actor's properties ASimModeBase(); virtual void BeginPlay() override; @@ -42,7 +55,6 @@ class AIRSIM_API ASimModeBase : public AActor virtual void Tick( float DeltaSeconds ) override; //additional overridable methods - virtual void reset(); virtual std::string getDebugReport(); virtual ECameraDirectorMode getInitialViewMode() const; @@ -110,6 +122,7 @@ class AIRSIM_API ASimModeBase : public AActor UPROPERTY() UClass* pip_camera_class; UPROPERTY() UParticleSystem* collision_display_template; + private: typedef common_utils::Utils Utils; typedef msr::airlib::ClockFactory ClockFactory; @@ -121,6 +134,7 @@ class AIRSIM_API ASimModeBase : public AActor UPROPERTY() UClass* external_camera_class_; UPROPERTY() UClass* camera_director_class_; UPROPERTY() UClass* sky_sphere_class_; + UPROPERTY() ULoadingScreenWidget* loading_screen_widget_; UPROPERTY() AActor* sky_sphere_; @@ -147,7 +161,7 @@ class AIRSIM_API ASimModeBase : public AActor bool lidar_checks_done_ = false; bool lidar_draw_debug_points_ = false; - + static ASimModeBase* SIMMODE; private: void setStencilIDs(); void initializeTimeOfDay(); @@ -156,4 +170,4 @@ class AIRSIM_API ASimModeBase : public AActor void setupPhysicsLoopPeriod(); void showClockStats(); void drawLidarDebugPoints(); -}; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 3f885d178b..7180fba12a 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -1,13 +1,137 @@ #include "WorldSimApi.h" +#include "common/common_utils/Utils.hpp" #include "AirBlueprintLib.h" #include "TextureShuffleActor.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" #include "DrawDebugHelpers.h" +#include +#include WorldSimApi::WorldSimApi(ASimModeBase* simmode) - : simmode_(simmode) + : simmode_(simmode) {} + +bool WorldSimApi::loadLevel(const std::string& level_name) +{ + bool success; + using namespace std::chrono_literals; + + // Add loading screen to viewport + simmode_->toggleLoadingScreen(true); + std::this_thread::sleep_for(0.1s); + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + + this->current_level_ = UAirBlueprintLib::loadLevel(this->simmode_->GetWorld(), FString(level_name.c_str())); + }, true); + + if (this->current_level_) + { + success = true; + std::this_thread::sleep_for(1s); + spawnPlayer(); + } + else + success = false; + + //Remove Loading screen from viewport + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + this->simmode_->OnLevelLoaded.Broadcast(); + }, true); + this->simmode_->toggleLoadingScreen(false); + + return success; +} + +void WorldSimApi::spawnPlayer() +{ + using namespace std::chrono_literals; + UE_LOG(LogTemp, Log, TEXT("spawning player")); + bool success{ false }; + + UAirBlueprintLib::RunCommandOnGameThread([&]() { + success = UAirBlueprintLib::spawnPlayer(this->simmode_->GetWorld()); + }, true); + + if (!success) + { + UE_LOG(LogTemp, Error, TEXT("Could not find valid PlayerStart Position")); + } + else + { + std::this_thread::sleep_for(1s); + simmode_->reset(); + } +} + +bool WorldSimApi::destroyObject(const std::string& object_name) +{ + bool result{ false }; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) + { + actor->Destroy(); + result = actor->IsPendingKill(); + } + }, true); + return result; +} + +std::string WorldSimApi::spawnObject(std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) { + // Create struct for Location and Rotation of actor in Unreal + FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose); + bool found_object; + UAirBlueprintLib::RunCommandOnGameThread([this, load_object, &object_name, &actor_transform, &found_object, &scale]() { + // Find mesh in /Game and /AirSim asset registry. When more plugins are added this function will have to change + UStaticMesh* LoadObject = dynamic_cast(UAirBlueprintLib::GetMeshFromRegistry(load_object)); + if (LoadObject) + { + std::vector matching_names = UAirBlueprintLib::ListMatchingActors(simmode_->GetWorld(), ".*"+object_name+".*"); + if (matching_names.size() > 0) + { + size_t greatest_num{ 0 }, result{ 0 }; + for (auto match : matching_names) + { + std::string number_extension = match.substr(match.find_last_not_of("0123456789") + 1); + if (number_extension != "") + { + result = std::stoi(number_extension); + greatest_num = greatest_num > result ? greatest_num : result; + } + } + object_name += std::to_string(greatest_num + 1); + } + FActorSpawnParameters new_actor_spawn_params; + new_actor_spawn_params.Name = FName(object_name.c_str()); + this->createNewActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + found_object = true; + } + else + { + found_object = false; + } + }, true); + + if (!found_object) + { + throw std::invalid_argument( + "There were no objects with name " + load_object + " found in the Registry"); + } + return object_name; +} + +void WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh) +{ + AActor* NewActor = simmode_->GetWorld()->SpawnActor(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, spawn_params); // new + UStaticMeshComponent* ObjectComponent = NewObject(NewActor); + ObjectComponent->SetStaticMesh(static_mesh); + ObjectComponent->SetRelativeLocation(FVector(0, 0, 0)); + ObjectComponent->SetWorldScale3D(FVector(scale[0], scale[1], scale[2])); + ObjectComponent->SetHiddenInGame(false, true); + ObjectComponent->RegisterComponent(); + NewActor->SetRootComponent(ObjectComponent); + NewActor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics); } bool WorldSimApi::isPaused() const @@ -80,6 +204,18 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con result = actor ? simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation())) : Pose::nanPose(); }, true); + + return result; +} + +WorldSimApi::Vector3r WorldSimApi::getObjectScale(const std::string& object_name) const +{ + Vector3r result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + result = actor ? Vector3r(actor->GetActorScale().X, actor->GetActorScale().Y, actor->GetActorScale().Z) + : Vector3r::Zero(); + }, true); return result; } @@ -101,6 +237,21 @@ bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimAp return result; } +bool WorldSimApi::setObjectScale(const std::string& object_name, const Vector3r& scale) +{ + bool result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &scale, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) { + actor->SetActorScale3D(FVector(scale[0], scale[1], scale[2])); + result = true; + } + else + result = false; + }, true); + return result; +} + void WorldSimApi::enableWeather(bool enable) { UWeatherLib::setWeatherEnabled(simmode_->GetWorld(), enable); @@ -116,41 +267,42 @@ void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) std::unique_ptr> WorldSimApi::swapTextures(const std::string& tag, int tex_id, int component_id, int material_id) { - auto swappedObjectNames = std::make_unique>(); - UAirBlueprintLib::RunCommandOnGameThread([this, &tag, tex_id, component_id, material_id, &swappedObjectNames]() { - //Split the tag string into individual tags. - TArray splitTags; - FString notSplit = FString(tag.c_str()); - FString next = ""; - while (notSplit.Split(",", &next, ¬Split)) - { - next.TrimStartInline(); - splitTags.Add(next); - } - notSplit.TrimStartInline(); - splitTags.Add(notSplit); - - //Texture swap on actors that have all of those tags. - TArray shuffleables; - UAirBlueprintLib::FindAllActor(simmode_, shuffleables); - for (auto *shuffler : shuffleables) - { - bool invalidChoice = false; - for (auto required_tag : splitTags) - { - invalidChoice |= !shuffler->ActorHasTag(FName(*required_tag)); - if (invalidChoice) - break; - } - - if (invalidChoice) - continue; - dynamic_cast(shuffler)->SwapTexture(tex_id, component_id, material_id); - swappedObjectNames->push_back(TCHAR_TO_UTF8(*shuffler->GetName())); - } - }, true); - return swappedObjectNames; + auto swappedObjectNames = std::make_unique>(); + UAirBlueprintLib::RunCommandOnGameThread([this, &tag, tex_id, component_id, material_id, &swappedObjectNames]() { + //Split the tag string into individual tags. + TArray splitTags; + FString notSplit = FString(tag.c_str()); + FString next = ""; + while (notSplit.Split(",", &next, ¬Split)) + { + next.TrimStartInline(); + splitTags.Add(next); + } + notSplit.TrimStartInline(); + splitTags.Add(notSplit); + + //Texture swap on actors that have all of those tags. + TArray shuffleables; + UAirBlueprintLib::FindAllActor(simmode_, shuffleables); + for (auto *shuffler : shuffleables) + { + bool invalidChoice = false; + for (auto required_tag : splitTags) + { + invalidChoice |= !shuffler->ActorHasTag(FName(*required_tag)); + if (invalidChoice) + break; + } + + if (invalidChoice) + continue; + dynamic_cast(shuffler)->SwapTexture(tex_id, component_id, material_id); + swappedObjectNames->push_back(TCHAR_TO_UTF8(*shuffler->GetName())); + } + }, true); + return swappedObjectNames; } + //----------- Plotting APIs ----------/ void WorldSimApi::simFlushPersistentMarkers() { diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 2f295968b8..7b647af934 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -4,17 +4,25 @@ #include "common/CommonStructs.hpp" #include "api/WorldSimApiBase.hpp" #include "SimMode/SimModeBase.h" +#include "Components/StaticMeshComponent.h" +#include "Runtime/Engine/Classes/Engine/StaticMesh.h" +#include "Engine/LevelStreamingDynamic.h" #include class WorldSimApi : public msr::airlib::WorldSimApiBase { public: typedef msr::airlib::Pose Pose; typedef msr::airlib::Vector3r Vector3r; - typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; + typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; + virtual bool loadLevel(const std::string& level_name) override; + + virtual std::string spawnObject(std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) override; + virtual bool destroyObject(const std::string& object_name) override; + virtual bool isPaused() const override; virtual void reset() override; virtual void pause(bool is_paused) override; @@ -32,10 +40,12 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; + virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; + virtual Vector3r getObjectScale(const std::string& object_name) const override; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) override; //----------- Plotting APIs ----------/ virtual void simFlushPersistentMarkers() override; @@ -46,13 +56,18 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) override; virtual void simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; virtual void simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) override; - virtual std::vector getMeshPositionVertexBuffers() const override; + virtual std::vector getMeshPositionVertexBuffers() const override; // Recording APIs virtual void startRecording() override; virtual void stopRecording() override; virtual bool isRecording() const override; +private: + void createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); + void spawnPlayer(); + private: ASimModeBase* simmode_; + ULevelStreamingDynamic* current_level_; };