diff --git a/Unreal/Environments/Blocks/Config/DefaultEngine.ini b/Unreal/Environments/Blocks/Config/DefaultEngine.ini index 7fe134840..851a90d6f 100644 --- a/Unreal/Environments/Blocks/Config/DefaultEngine.ini +++ b/Unreal/Environments/Blocks/Config/DefaultEngine.ini @@ -68,4 +68,17 @@ AsyncSceneSmoothingFactor=0.990000 InitialAverageFrameRate=0.016667 PhysXTreeRebuildRate=10 +[/Script/AndroidFileServerEditor.AndroidFileServerRuntimeSettings] +bEnablePlugin=True +bAllowNetworkConnection=True +SecurityToken=A21DA9C4419D17AEC507B8A8664AC14B +bIncludeInShipping=False +bAllowExternalStartInShipping=False +bCompileAFSProject=False +bUseCompression=False +bLogFiles=False +bReportStats=False +ConnectionType=USBOnly +bUseManualIPAddress=False +ManualIPAddress= diff --git a/Unreal/Environments/Blocks/Config/DefaultInput.ini b/Unreal/Environments/Blocks/Config/DefaultInput.ini index 78803ce4f..c68d330fd 100644 --- a/Unreal/Environments/Blocks/Config/DefaultInput.ini +++ b/Unreal/Environments/Blocks/Config/DefaultInput.ini @@ -1,20 +1,86 @@ [/Script/Engine.InputSettings] +-AxisConfig=(AxisKeyName="Gamepad_LeftX",AxisProperties=(DeadZone=0.25,Exponent=1.f,Sensitivity=1.f)) +-AxisConfig=(AxisKeyName="Gamepad_LeftY",AxisProperties=(DeadZone=0.25,Exponent=1.f,Sensitivity=1.f)) +-AxisConfig=(AxisKeyName="Gamepad_RightX",AxisProperties=(DeadZone=0.25,Exponent=1.f,Sensitivity=1.f)) +-AxisConfig=(AxisKeyName="Gamepad_RightY",AxisProperties=(DeadZone=0.25,Exponent=1.f,Sensitivity=1.f)) +-AxisConfig=(AxisKeyName="MouseX",AxisProperties=(DeadZone=0.f,Exponent=1.f,Sensitivity=0.07f)) +-AxisConfig=(AxisKeyName="MouseY",AxisProperties=(DeadZone=0.f,Exponent=1.f,Sensitivity=0.07f)) +-AxisConfig=(AxisKeyName="Mouse2D",AxisProperties=(DeadZone=0.f,Exponent=1.f,Sensitivity=0.07f)) ++AxisConfig=(AxisKeyName="Gamepad_LeftX",AxisProperties=(DeadZone=0.250000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_LeftY",AxisProperties=(DeadZone=0.250000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_RightX",AxisProperties=(DeadZone=0.250000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_RightY",AxisProperties=(DeadZone=0.250000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MouseX",AxisProperties=(DeadZone=0.000000,Sensitivity=0.070000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MouseY",AxisProperties=(DeadZone=0.000000,Sensitivity=0.070000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Mouse2D",AxisProperties=(DeadZone=0.000000,Sensitivity=0.070000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MouseWheelAxis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_LeftTriggerAxis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_RightTriggerAxis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_Special_Left_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Gamepad_Special_Left_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Left_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Left_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Left_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Right_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Right_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="Vive_Right_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Left_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Left_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Left_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Left_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Left_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Right_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Right_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Right_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Right_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="MixedReality_Right_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Left_Grip_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Left_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Left_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Left_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Right_Grip_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Right_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Right_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="OculusTouch_Right_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Grip_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Grip_Force",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Left_Trackpad_Force",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Grip_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Grip_Force",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Trigger_Axis",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Thumbstick_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Thumbstick_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Trackpad_X",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Trackpad_Y",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) ++AxisConfig=(AxisKeyName="ValveIndex_Right_Trackpad_Force",AxisProperties=(DeadZone=0.000000,Sensitivity=1.000000,Exponent=1.000000,bInvert=False)) bAltEnterTogglesFullscreen=True bF11TogglesFullscreen=True bUseMouseForTouch=False bEnableMouseSmoothing=True bEnableFOVScaling=True -FOVScale=0.011110 -DoubleClickTime=0.200000 bCaptureMouseOnLaunch=False -DefaultViewportMouseCaptureMode=NoCapture -bDefaultViewportMouseLock=False -DefaultViewportMouseLockMode=DoNotLock +bEnableLegacyInputScales=True +bEnableMotionControls=True +bFilterInputByPlatformUser=False +bEnableInputDeviceSubsystem=True +bShouldFlushPressedKeysOnViewportFocusLost=True +bEnableDynamicComponentInputBinding=True bAlwaysShowTouchInterface=False bShowConsoleOnFourFingerTap=True +bEnableGestureRecognizer=False +bUseAutocorrect=False +DefaultViewportMouseCaptureMode=NoCapture +DefaultViewportMouseLockMode=DoNotLock +FOVScale=0.011110 +DoubleClickTime=0.200000 +DefaultPlayerInputClass=/Script/EnhancedInput.EnhancedPlayerInput +DefaultInputComponentClass=/Script/EnhancedInput.EnhancedInputComponent DefaultTouchInterface=/Engine/MobileResources/HUD/DefaultVirtualJoysticks.DefaultVirtualJoysticks -ConsoleKey=None -ConsoleKeys=Tilde +ConsoleKeys=Tilde - diff --git a/Unreal/Plugins/AirSim/Source/Beacons/DynamicBlockBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/DynamicBlockBeacon.cpp index 1170190c2..51b43c402 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/DynamicBlockBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/DynamicBlockBeacon.cpp @@ -15,8 +15,9 @@ ADynamicBlockBeacon::ADynamicBlockBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Passive Source Mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh(TEXT("StaticMesh'/AirSim/Beacons/DynamicBlockBeacon.DynamicBlockBeacon'")); diff --git a/Unreal/Plugins/AirSim/Source/Beacons/DynamicRackBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/DynamicRackBeacon.cpp index 508c6ddf8..a7bcc9ff4 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/DynamicRackBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/DynamicRackBeacon.cpp @@ -15,8 +15,9 @@ ADynamicRackBeacon::ADynamicRackBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Beacon mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh(TEXT("StaticMesh'/AirSim/Beacons/DynamicRackBeacon.DynamicRackBeacon'")); diff --git a/Unreal/Plugins/AirSim/Source/Beacons/FiducialBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/FiducialBeacon.cpp index 8e9e981f4..2c8db30f0 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/FiducialBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/FiducialBeacon.cpp @@ -18,8 +18,9 @@ AFiducialBeacon::AFiducialBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Beacon mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh0(TEXT("StaticMesh'/AirSim/Beacons/fiducials/aruco.aruco'")); diff --git a/Unreal/Plugins/AirSim/Source/Beacons/TemplateBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/TemplateBeacon.cpp index 7fdfe5e8e..e92675655 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/TemplateBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/TemplateBeacon.cpp @@ -15,8 +15,9 @@ ATemplateBeacon::ATemplateBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Beacon mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh(TEXT("StaticMesh'/AirSim/Beacons/templateBeacon.templateBeacon'")); diff --git a/Unreal/Plugins/AirSim/Source/Beacons/UWBBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/UWBBeacon.cpp index c8c60afcc..da92ff321 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/UWBBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/UWBBeacon.cpp @@ -14,8 +14,9 @@ AUWBBeacon::AUWBBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Beacon mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh(TEXT("StaticMesh'/AirSim/Beacons/uwbBeacon.uwbBeacon'")); diff --git a/Unreal/Plugins/AirSim/Source/Beacons/WifiBeacon.cpp b/Unreal/Plugins/AirSim/Source/Beacons/WifiBeacon.cpp index 71c3418f0..ff22fdb7d 100644 --- a/Unreal/Plugins/AirSim/Source/Beacons/WifiBeacon.cpp +++ b/Unreal/Plugins/AirSim/Source/Beacons/WifiBeacon.cpp @@ -15,8 +15,9 @@ AWifiBeacon::AWifiBeacon() // set the mesh Mesh = CreateDefaultSubobject(TEXT("Beacon mesh")); + this->SetRootComponent(Mesh); // attach to root component - Mesh->SetupAttachment(GetRootComponent()); + //Mesh->SetupAttachment(GetRootComponent()); // set path for static mesh static ConstructorHelpers::FObjectFinder loadedMesh(TEXT("StaticMesh'/AirSim/Beacons/wifiBeacon.wifiBeacon'")); diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp index 6cdd8aa27..a9f83a9ff 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.cpp +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.cpp @@ -8,6 +8,7 @@ ACameraDirector::ACameraDirector() // Create a spring arm component for our chase camera SpringArm = CreateDefaultSubobject(TEXT("SpringArm")); + this->SetRootComponent(SpringArm); SpringArm->SetRelativeLocation(FVector(0.0f, 0.0f, 34.0f)); SpringArm->SetWorldRotation(FRotator(-20.0f, 0.0f, 0.0f)); SpringArm->TargetArmLength = 125.0f; diff --git a/Unreal/Plugins/AirSim/Source/DetectionComponent.h b/Unreal/Plugins/AirSim/Source/DetectionComponent.h index 6f0899af1..6973a3b6e 100644 --- a/Unreal/Plugins/AirSim/Source/DetectionComponent.h +++ b/Unreal/Plugins/AirSim/Source/DetectionComponent.h @@ -5,6 +5,7 @@ #include "CoreMinimal.h" #include "Components/SceneComponent.h" #include "ObjectFilter.h" +#include #include "DetectionComponent.generated.h" USTRUCT() diff --git a/Unreal/Plugins/AirSim/Source/LidarCamera.cpp b/Unreal/Plugins/AirSim/Source/LidarCamera.cpp index 7065fec56..1a4291834 100755 --- a/Unreal/Plugins/AirSim/Source/LidarCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/LidarCamera.cpp @@ -67,6 +67,7 @@ ALidarCamera::ALidarCamera() // Add components arrow_ = CreateDefaultSubobject(TEXT("Arrow")); + this->SetRootComponent(arrow_); capture_2D_depth_ = CreateDefaultSubobject(TEXT("Lidar2DDepth")); capture_2D_segmentation_ = CreateDefaultSubobject(TEXT("Lidar2DSegmentation")); capture_2D_intensity_ = CreateDefaultSubobject(TEXT("Lidar2DIntensity")); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 8ad618d2b..0906bd502 100755 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -477,7 +477,7 @@ void ASimModeBase::Tick(float DeltaSeconds) updateDebugReport(debug_reporter_); - drawLidarDebugPoints(); + //drawLidarDebugPoints(); drawDistanceSensorDebugPoints(); @@ -813,6 +813,112 @@ void ASimModeBase::setupVehiclesAndCamera() } } } + + + //add beacons from settings + for (auto const& beacon_setting_pair : getSettings().beacons) + { + //if vehicle is of type for derived SimMode and auto creatable + const auto& beacon_setting = *beacon_setting_pair.second; + //if (beacon_setting.auto_create && + //isVehicleTypeSupported(beacon_setting.beacon_type)) { + if (beacon_setting.auto_create) { + //compute initial pose + FVector spawn_position = uu_origin.GetLocation(); + msr::airlib::Vector3r settings_position = beacon_setting.position; + /*FVector globalOffset = getGlobalNedTransform().getGlobalOffset(); + + settings_position.x() += globalOffset.X; + settings_position.y() += globalOffset.Y; + settings_position.z() += globalOffset.Z;*/ + + + if (!msr::airlib::VectorMath::hasNan(settings_position)) + spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); + FRotator spawn_rotation = toFRotator(beacon_setting.rotation, uu_origin.Rotator()); + + //spawn beacon pawn + FActorSpawnParameters pawn_spawn_params; + FString comboName = beacon_setting.beacon_name.c_str() + FString(":") + beacon_setting.beacon_pawn_name.c_str(); + pawn_spawn_params.Name = FName(*comboName); + pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + //auto beacon_bp_class = UAirBlueprintLib::LoadClass(getSettings().pawn_paths.at(beacon_setting.beacon_pawn_name).pawn_bp); + //FActorSpawnParameters SpawnInfo; + // TODO: Make the child sim modes responsible for casting the types. + //ATemplateBeacon* spawned_beacon = static_cast(this->GetWorld()->SpawnActor(beacon_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params2)); + + if (beacon_setting.beacon_type.compare("fiducialmarker") == 0) { + AFiducialBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + spawned_beacon->setScale(beacon_setting.scale); + } + else if (beacon_setting.beacon_type.compare("uwbbeacon") == 0) { + AUWBBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + } + else if (beacon_setting.beacon_type.compare("wifibeacon") == 0) { + AWifiBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + } + else if (beacon_setting.beacon_type.compare("dynamicblockbeacon") == 0) { + ADynamicBlockBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + } + else if (beacon_setting.beacon_type.compare("dynamicrackbeacon") == 0) { + ADynamicRackBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + } + else { + ATemplateBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); + } + + //this->GetWorld()->SpawnActor(ATemplateBeacon) + /*AActor* spawned_actor = static_cast(this->GetWorld()->SpawnActor( + beacon_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params2)); + + AirsimVehicle* spawned_pawn2 = dynamic_cast(spawned_actor); + + spawned_actors_.Add(spawned_pawn2->GetPawn()); + pawns.Add(spawned_pawn2); + + if (beacon_setting.is_fpv_vehicle) + fpv_pawn = spawned_pawn2->GetPawn();*/ + } + } + + //add passive echo beacons from settings + for (auto const& passive_echo_beacon_setting_pair : getSettings().passive_echo_beacons) + { + const auto& passive_echo_beacon_setting = *passive_echo_beacon_setting_pair.second; + //compute initial pose + FVector spawn_position = FVector(0, 0, 0); + msr::airlib::Vector3r settings_position = passive_echo_beacon_setting.position; + if (!msr::airlib::VectorMath::hasNan(settings_position)) + spawn_position = global_ned_transform_->toFVector(settings_position, 100, true); + FRotator spawn_rotation = toFRotator(passive_echo_beacon_setting.rotation, FRotator()); + + //spawn passive echo beacon actor + FActorSpawnParameters actor_spawn_params; + actor_spawn_params.Name = FName(passive_echo_beacon_setting.name.c_str()); + actor_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + actor_spawn_params.bDeferConstruction = true; + APassiveEchoBeacon* spawned_passive_echo_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, actor_spawn_params)); + spawned_passive_echo_beacon->enable_ = passive_echo_beacon_setting.enable; + spawned_passive_echo_beacon->initial_directions_ = passive_echo_beacon_setting.initial_directions; + spawned_passive_echo_beacon->initial_lower_azimuth_limit_ = passive_echo_beacon_setting.initial_lower_azimuth_limit; + spawned_passive_echo_beacon->initial_upper_azimuth_limit_ = passive_echo_beacon_setting.initial_upper_azimuth_limit; + spawned_passive_echo_beacon->initial_lower_elevation_limit_ = passive_echo_beacon_setting.initial_lower_elevation_limit; + spawned_passive_echo_beacon->initial_upper_elevation_limit_ = passive_echo_beacon_setting.initial_upper_elevation_limit; + spawned_passive_echo_beacon->attenuation_limit_ = passive_echo_beacon_setting.attenuation_limit; + spawned_passive_echo_beacon->reflection_distance_limit_ = passive_echo_beacon_setting.reflection_distance_limit; + spawned_passive_echo_beacon->reflection_only_final_ = passive_echo_beacon_setting.reflection_only_final; + spawned_passive_echo_beacon->attenuation_per_distance_ = passive_echo_beacon_setting.attenuation_per_distance; + spawned_passive_echo_beacon->attenuation_per_reflection_ = passive_echo_beacon_setting.attenuation_per_reflection; + spawned_passive_echo_beacon->distance_limit_ = passive_echo_beacon_setting.distance_limit; + spawned_passive_echo_beacon->reflection_limit_ = passive_echo_beacon_setting.reflection_limit; + spawned_passive_echo_beacon->draw_debug_location_ = passive_echo_beacon_setting.draw_debug_location; + spawned_passive_echo_beacon->draw_debug_all_points_ = passive_echo_beacon_setting.draw_debug_all_points; + spawned_passive_echo_beacon->draw_debug_all_lines_ = passive_echo_beacon_setting.draw_debug_all_lines; + spawned_passive_echo_beacon->draw_debug_duration_ = passive_echo_beacon_setting.draw_debug_duration; + spawned_passive_echo_beacon->FinishSpawning(FTransform(spawn_rotation, spawn_position)); + } + //create API objects for each pawn we have for (AActor* pawn : pawns) { auto vehicle_pawn = static_cast(pawn); @@ -859,6 +965,13 @@ std::string ASimModeBase::getVehiclePawnPathName(const AirSimSettings::VehicleSe //derived class should override this method to retrieve types of pawns they support return ""; } + +std::string ASimModeBase::getBeaconPawnPathName(const AirSimSettings::BeaconSetting& beacon_setting) const +{ + //derived class should override this method to retrieve types of pawns they support + return ""; +} + PawnEvents* ASimModeBase::getVehiclePawnEvents(APawn* pawn) const { unused(pawn); @@ -931,10 +1044,10 @@ void ASimModeBase::drawLidarDebugPoints() msr::airlib::Vector3r point(lidar_data.point_cloud[j], lidar_data.point_cloud[j + 1], lidar_data.point_cloud[j + 2]); FVector uu_point; - if (lidar_data.point_cloud[j] == 0 && lidar_data.point_cloud[j + 1] == 0 && lidar_data.point_cloud[j + 2] == 0) { - } - else { - if (lidar->getParams().data_frame == AirSimSettings::kVehicleInertialFrame) { + if (lidar_data.point_cloud[j] == 0 && lidar_data.point_cloud[j + 1] == 0 && lidar_data.point_cloud[j + 2] == 0) { + } + else { + if (lidar->getParams().data_frame == AirSimSettings::kVehicleInertialFrame) { if (lidar->getParams().external) { msr::airlib::Vector3r point_w = msr::airlib::VectorMath::transformToWorldFrame(point, lidar_data.pose, true); @@ -943,10 +1056,10 @@ void ASimModeBase::drawLidarDebugPoints() else { uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point); } - } - else if (lidar->getParams().data_frame == AirSimSettings::kSensorLocalFrame) { + } + else if (lidar->getParams().data_frame == AirSimSettings::kSensorLocalFrame) { - msr::airlib::Vector3r point_w = msr::airlib::VectorMath::transformToWorldFrame(point, lidar_data.pose, true); + msr::airlib::Vector3r point_w = msr::airlib::VectorMath::transformToWorldFrame(point, lidar_data.pose, true); if (lidar->getParams().external) { uu_point = pawn_sim_api->getNedTransform().toFVector(point_w, 100, true); @@ -954,19 +1067,19 @@ void ASimModeBase::drawLidarDebugPoints() else { uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point_w); } - } - else - throw std::runtime_error("Unknown requested data frame"); - - UAirBlueprintLib::DrawPoint( - this->GetWorld(), - uu_point, - 5, //size - FColor::Green, - false, //persistent (never goes away) - 0.1 //point leaves a trail on moving object - ); - } + } + else + throw std::runtime_error("Unknown requested data frame"); + + UAirBlueprintLib::DrawPoint( + this->GetWorld(), + uu_point, + 5, //size + FColor::Green, + false, //persistent (never goes away) + 0.1 //point leaves a trail on moving object + ); + } } } } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp index 50f54a6a1..f950f9a50 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp @@ -327,7 +327,7 @@ void ACarPawn::updatePhysicsMaterial() //This method must be in pawn because Unreal doesn't allow key bindings to non UObject pointers void ACarPawn::setupInputBindings() { - UAirBlueprintLib::EnableInput(this); + //UAirBlueprintLib::EnableInput(this); UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveForward", EKeys::Up, 1), this, this, &ACarPawn::onMoveForward); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.cpp index 8df4b3f0a..1ff5de0f9 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.cpp @@ -52,26 +52,6 @@ float FVehicleEngineDataSkid::FindPeakTorque() const USkidVehicleMovementComponent::USkidVehicleMovementComponent(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) { - RightThrustRate.FallRate = 5.f; - RightThrustRate.InputCurveFunction = EInputFunctionType::LinearFunction; - RightThrustRate.RiseRate = 2.5f; - LeftThrustRate.FallRate = 5.f; - LeftThrustRate.RiseRate = 2.5f; - LeftThrustRate.InputCurveFunction = EInputFunctionType::LinearFunction; - LeftBrakeRate.RiseRate = 6.f; - LeftBrakeRate.FallRate = 10.f; - LeftBrakeRate.InputCurveFunction = EInputFunctionType::LinearFunction; - RightBrakeRate.RiseRate = 6.f; - RightBrakeRate.FallRate = 10.f; - RightBrakeRate.InputCurveFunction = EInputFunctionType::LinearFunction; - - // Init steering speed curve - FRichCurve* SteeringCurveData = SteeringCurve.GetRichCurve(); - SteeringCurveData->AddKey(0.f, 1.f); - SteeringCurveData->AddKey(20.f, 0.9f); - SteeringCurveData->AddKey(60.f, 0.8f); - SteeringCurveData->AddKey(120.f, 0.7f); - // Initialize WheelSetups array with 4 wheels WheelSetups.SetNum(4); } @@ -117,140 +97,29 @@ void USkidVehicleMovementComponent::SetupVehicle(TUniquePtr= 0) { - // Forward - nMotPremixL = (nJoyX >= 0) ? 1 : (1 + nJoyX); - nMotPremixR = (nJoyX >= 0) ? (1 - nJoyX) : 1; - } - else { - // Reverse - nMotPremixL = (nJoyX >= 0) ? (1 - nJoyX) : 1; - nMotPremixR = (nJoyX >= 0) ? 1 : (1 + nJoyX); - } - - // Scale Drive output due to Joystick Y input (throttle) - nMotPremixL = nMotPremixL * nJoyY / 1; - nMotPremixR = nMotPremixR * nJoyY / 1; - - // Now calculate pivot amount - // - Strength of pivot (nPivSpeed) based on Joystick X input - // - Blending of pivot vs drive (fPivScale) based on Joystick Y input - nPivSpeed = nJoyX; - fPivScale = (abs(nJoyY) >= fPivYLimit) ? 0.0 : (1.0 - abs(nJoyY) / fPivYLimit); - - // Calculate final mix of Drive and Pivot - nMotMixL = (1.0 - fPivScale)*nMotPremixL + fPivScale * (nPivSpeed); - nMotMixR = (1.0 - fPivScale)*nMotPremixR + fPivScale * (-nPivSpeed); - - - FChaosVehicleDefaultAsyncInput VehicleInputData; - VehicleInputData.ControlInputs.YawInput = nJoyX; - VehicleInputData.ControlInputs.ThrottleInput = accel; - - if (!Super::GetUseAutoGears()) - { - VehicleInputData.ControlInputs.GearUpInput = bRawGearUpInput; - VehicleInputData.ControlInputs.GearDownInput = bRawGearDownInput; - } - - // Toggle on/off digital (hand) break - if (toggleBreak) { - VehicleInputData.ControlInputs.BrakeInput = LeftBreak; - VehicleInputData.ControlInputs.ThrottleInput = 0; - VehicleInputData.ControlInputs.HandbrakeInput = LeftBreak; - //UE_LOG(LogTemp, Warning, TEXT("Braking %f %f "), VehicleInputData.getAnalogLeftBrake(), VehicleInputData.getAnalogRightBrake()); - } - VehicleSimulationPT->UpdateSimulation(DeltaTime, VehicleInputData, Handle); -} - #if WITH_EDITOR void USkidVehicleMovementComponent::PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent) { Super::PostEditChangeProperty(PropertyChangedEvent); - const FName PropertyName = PropertyChangedEvent.Property ? PropertyChangedEvent.Property->GetFName() : NAME_None; - - if (PropertyName == TEXT("DownRatio")) - { - for (int32 GearIdx = 0; GearIdx < TransmissionSetupLocal.ForwardGears.Num(); ++GearIdx) - { - FVehicleGearDataSkid& GearData = TransmissionSetupLocal.ForwardGears[GearIdx]; - GearData.DownRatio = FMath::Min(GearData.DownRatio, GearData.UpRatio); - } - } - else if (PropertyName == TEXT("UpRatio")) - { - for (int32 GearIdx = 0; GearIdx < TransmissionSetupLocal.ForwardGears.Num(); ++GearIdx) - { - FVehicleGearDataSkid& GearData = TransmissionSetupLocal.ForwardGears[GearIdx]; - GearData.UpRatio = FMath::Max(GearData.DownRatio, GearData.UpRatio); - } - } - else if (PropertyName == TEXT("SteeringCurve")) - { - //make sure values are capped between 0 and 1 - TArray SteerKeys = SteeringCurve.GetRichCurve()->GetCopyOfKeys(); - for (int32 KeyIdx = 0; KeyIdx < SteerKeys.Num(); ++KeyIdx) - { - float NewValue = FMath::Clamp(SteerKeys[KeyIdx].Value, 0.f, 1.f); - SteeringCurve.GetRichCurve()->UpdateOrAddKey(SteerKeys[KeyIdx].Time, NewValue); - } - } } #endif void USkidVehicleMovementComponent::SetAcceleration(float OtherAcceleration) { - this->Acceleration = OtherAcceleration; + this->Acceleration = OtherAcceleration; } void USkidVehicleMovementComponent::SetLeftBreak(float OtherLeftBreak) { this->LeftBreak = OtherLeftBreak; + SetBrakeInput(GetLeftBreak()); } void USkidVehicleMovementComponent::SetRightBreak(float OtherRightBreak) { this->RightBreak = OtherRightBreak; + SetBrakeInput(GetRightBreak()); + } void USkidVehicleMovementComponent::SetYJoy(float OtherNJoyY) @@ -261,6 +130,9 @@ void USkidVehicleMovementComponent::SetYJoy(float OtherNJoyY) void USkidVehicleMovementComponent::SetXJoy(float OtherNJoyX) { this->nJoyX = OtherNJoyX; + float accel = FMath::Min((float)sqrt(nJoyX * nJoyX + nJoyY * nJoyY), 1.0f); + SetThrottleInput(GetAcceleration()); + SetYawInput(nJoyX); } float USkidVehicleMovementComponent::GetAcceleration() const @@ -291,9 +163,13 @@ float USkidVehicleMovementComponent::GetXJoy() const void USkidVehicleMovementComponent::SetBreaksOn() { this->toggleBreak = true; + SetAcceleration(0); + SetLeftBreak(1); + SetParked(1); } void USkidVehicleMovementComponent::SetBreaksOff() { this->toggleBreak = false; + SetParked(0); } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.h b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.h index ef482678b..3b8187e32 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehicleMovementComponent.h @@ -168,29 +168,8 @@ class AIRSIM_API USkidVehicleMovementComponent : public UChaosWheeledVehicleMove public: - /** Engine */ - UPROPERTY(EditAnywhere, Category = MechanicalSetup) - FVehicleEngineDataSkid EngineSetupLocal; - - /** Transmission data */ - UPROPERTY(EditAnywhere, Category = MechanicalSetup) - FVehicleTransmissionDataSkid TransmissionSetupLocal; - UPROPERTY(EditAnywhere, Category = "Skid Setup") - uint32 NumOfWheels; - - /** Maximum steering versus forward speed (km/h) */ - UPROPERTY(EditAnywhere, Category = SteeringSetup) - FRuntimeFloatCurve SteeringCurve; - - UPROPERTY(EditAnywhere, Category = SkidInput, AdvancedDisplay) - FVehicleInputRateConfig LeftThrustRate; - UPROPERTY(EditAnywhere, Category = SkidInput, AdvancedDisplay) - FVehicleInputRateConfig RightThrustRate; - UPROPERTY(EditAnywhere, Category = SkidInput, AdvancedDisplay) - FVehicleInputRateConfig RightBrakeRate; - UPROPERTY(EditAnywhere, Category = SkidInput, AdvancedDisplay) - FVehicleInputRateConfig LeftBrakeRate; + uint32 NumOfWheels; public: @@ -202,8 +181,6 @@ class AIRSIM_API USkidVehicleMovementComponent : public UChaosWheeledVehicleMove virtual void SetupVehicle(TUniquePtr& PVehicle) override; - void UpdateSimulation(float DeltaTime, const FChaosVehicleDefaultAsyncInput& InputData, Chaos::FRigidBodyHandle_Internal* Handle); - protected: diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawn.cpp index 530c3e8b1..410353eb1 100755 --- a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawn.cpp @@ -266,7 +266,7 @@ void ASkidVehiclePawn::updatePhysicsMaterial() //This method must be in pawn because Unreal doesn't allow key bindings to non UObject pointers void ASkidVehiclePawn::setupInputBindings() { - UAirBlueprintLib::EnableInput(this); + //UAirBlueprintLib::EnableInput(this); UAirBlueprintLib::BindAxisToKey(FInputAxisKeyMapping("MoveY", EKeys::Up, 0.5), this, this, &ASkidVehiclePawn::onMoveY); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawnApi.cpp index f810c4312..2ab1329e7 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/SkidSteer/SkidVehiclePawnApi.cpp @@ -32,6 +32,13 @@ void SkidVehiclePawnApi::updateMovement(const msr::airlib::CarApiBase::CarContro movement_->SetBreaksOff(); } movement_->SetUseAutomaticGears(!controls.is_manual_gear); + + //float accel = FMath::Min((float)sqrt(controls.steering * controls.steering + controls.throttle * controls.throttle), 1.0f); + //movement_->SetThrottleInput(accel); + //movement_->SetSteeringInput(controls.steering); + //movement_->SetBrakeInput(controls.brake); + //movement_->SetHandbrakeInput(controls.handbrake); + //movement_->SetUseAutomaticGears(!controls.is_manual_gear); } msr::airlib::CarApiBase::CarState SkidVehiclePawnApi::getCarState() const