diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index f1292d6..ce51168 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -40,6 +40,8 @@ class RpcLibClientBase { Pose simGetObjectPose(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); + bool simSpawnStaticMeshObject(const std::string& object_class_name, const std::string& object_class, const Pose& pose); + bool simDeleteObject(const std::string& object_name); //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 5b5562d..1dc9ac1 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -26,6 +26,8 @@ class WorldSimApiBase { virtual Pose getObjectPose(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; + virtual bool spawnStaticMeshObject(const std::string& object_class_name, const std::string& object_name, const Pose &pose) = 0; + virtual bool deleteObject(const std::string& object_name) = 0; //----------- APIs to control ACharacter in scene ----------/ virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) = 0; diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index c945276..77dd8ce 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -593,7 +593,7 @@ class Utils { //implements relative method - do not use for comparing with zero //use this most of the time, tolerance needs to be meaningful in your context template - static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon()) + static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = std::numeric_limits::epsilon()) { TReal diff = std::fabs(a - b); if (diff <= tolerance) diff --git a/AirLib/include/common/common_utils/type_utils.hpp b/AirLib/include/common/common_utils/type_utils.hpp index 60a6fef..f5a3f1e 100644 --- a/AirLib/include/common/common_utils/type_utils.hpp +++ b/AirLib/include/common/common_utils/type_utils.hpp @@ -44,12 +44,14 @@ namespace common_utils { namespace type_utils { template static no & f(...); - template + // using template parameter symbol 'D' instead of 'C' as a workaround for + // VS2017 compiler issue (internal compiler error) starting 15.9.X releases. + template static yes & g(typename std::enable_if< - std::is_same(&C::end)), - typename C::const_iterator(C::*)() const>::value, void>::type*); + std::is_same(&D::end)), + typename D::const_iterator(D::*)() const>::value, void>::type*); - template static no & g(...); + template static no & g(...); public: static bool const beg_value = sizeof(f(nullptr)) == sizeof(yes); @@ -84,4 +86,4 @@ namespace common_utils { namespace type_utils { struct is_container> : std::true_type { }; }} //namespace -#endif +#endif \ No newline at end of file diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 9f15114..6b1b127 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -373,6 +373,14 @@ bool RpcLibClientBase::simSetObjectPose(const std::string& object_name, const ms { return pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose), teleport).as(); } +bool RpcLibClientBase::simSpawnStaticMeshObject(const std::string& object_class_name, const std::string& object_name, const msr::airlib::Pose& pose) +{ + return pimpl_->client.call("simSpawnStaticMeshObject", object_class_name, object_name, RpcLibAdapatorsBase::Pose(pose)).as(); +} +bool RpcLibClientBase::simDeleteObject(const std::string& object_name) +{ + return pimpl_->client.call("simDeleteObject", object_name).as(); +} CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const { diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 057f67b..611634b 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -185,6 +185,12 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& 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("simSpawnStaticMeshObject", [&](const std::string& object_class_name, const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose) -> bool { + return getWorldSimApi()->spawnStaticMeshObject(object_class_name, object_name, pose.to()); + }); + pimpl_->server.bind("simDeleteObject", [&](const std::string& object_name) -> bool { + return getWorldSimApi()->deleteObject(object_name); + }); pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); diff --git a/MavLinkCom/common_utils/type_utils.hpp b/MavLinkCom/common_utils/type_utils.hpp index 728dd02..5b52a34 100644 --- a/MavLinkCom/common_utils/type_utils.hpp +++ b/MavLinkCom/common_utils/type_utils.hpp @@ -44,12 +44,14 @@ namespace mavlink_utils { namespace type_utils { template static no & f(...); - template + // using template parameter symbol 'D' instead of 'C' as a workaround for + // VS2017 compiler issue (internal compiler error) starting 15.9.X releases. + template static yes & g(typename std::enable_if< - std::is_same(&C::end)), - typename C::const_iterator(C::*)() const>::value, void>::type*); + std::is_same(&D::end)), + typename D::const_iterator(D::*)() const>::value, void>::type*); - template static no & g(...); + template static no & g(...); public: static bool const beg_value = sizeof(f(nullptr)) == sizeof(yes); @@ -84,4 +86,4 @@ namespace mavlink_utils { namespace type_utils { struct is_container> : std::true_type { }; }} //namespace -#endif +#endif \ No newline at end of file diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 289566a..3062224 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - urdfbot\GenerateImageTrainingSet.py + urdfbot\LunabotClientTest.py . @@ -126,9 +126,6 @@ Code - - Code - Code @@ -147,6 +144,9 @@ Code + + Code + diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index b010ee3..773728a 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -156,6 +156,11 @@ def simGetObjectPose(self, object_name): def simSetObjectPose(self, object_name, pose, teleport = True): return self.client.call('simSetObjectPose', object_name, pose, teleport) + def simSpawnStaticMeshObject(self, object_class_name, object_name, pose): + return self.client.call('simSpawnStaticMeshObject', object_class_name, object_name, pose) + def simDeleteObject(self, object_name): + return self.client.call('simDeleteObject', object_name) + 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): diff --git a/PythonClient/urdfbot/SpawnObjectTest.py b/PythonClient/urdfbot/SpawnObjectTest.py new file mode 100644 index 0000000..2d307c8 --- /dev/null +++ b/PythonClient/urdfbot/SpawnObjectTest.py @@ -0,0 +1,63 @@ +import setup_path +import airsim +import airsim.airsim_types as at +import math +import time +import numpy as np +import pandas as pd +import random +import time + +def main(): + client = airsim.UrdfBotClient() + + random.seed(42) + + + cone_class_name = "/Game/StarterContent/Props/SM_Couch.SM_Couch" + + xs = [0, 0.1] + ys = [0, 0.1] + time_stamp = 0 + + for i in range(0, 3, 1): + xs.append(random.randint(0, 10)) + ys.append(random.randint(0, 10)) + + for i in range(0, len(xs), 1): + x = xs[i] + y = ys[i] + random_position = at.Vector3r(x_val=x, y_val=y, z_val=5) + random_orientation = at.Quaternionr(x_val=0, y_val=0, z_val=0, w_val=1) + random_pose = at.Pose(position_val=random_position, orientation_val=random_orientation) + + print('Spawning cone at ({0}, {1})...'.format(x, y)) + client.simSpawnStaticMeshObject(cone_class_name, 'cone_{0}'.format(i), random_pose) + + time.sleep(2) + ci = client.simGetCollisionInfo() + if (ci.has_collided and ci.time_stamp > time_stamp): + print('\tBot has collided with {0}.'.format(ci.object_name)) + time_stamp = ci.time_stamp + else: + print('\tBot has not collided.') + + time.sleep(3) + + for i in range(0, len(xs), 1): + cone_name = 'cone_{0}'.format(i) + cone_pose = client.simGetObjectPose(cone_name) + print('Cone "{0}" is at position <{1}, {2}, {3}>'.format(cone_name, cone_pose.position.x_val, cone_pose.position.y_val, cone_pose.position.z_val)) + + time.sleep(3) + + for i in range(0, len(xs), 1): + cone_name = 'cone_{0}'.format(i) + print('Deleting cone {0}...'.format(cone_name)) + client.simDeleteObject(cone_name) + + print('Graceful termination') + +main() + + diff --git a/Unreal/Environments/Blocks/Config/DefaultEngine.ini b/Unreal/Environments/Blocks/Config/DefaultEngine.ini index 55c5091..6b6ecf2 100644 --- a/Unreal/Environments/Blocks/Config/DefaultEngine.ini +++ b/Unreal/Environments/Blocks/Config/DefaultEngine.ini @@ -12,7 +12,7 @@ GlobalDefaultGameMode=/Script/AirSim.AirSimGameMode GlobalDefaultServerGameMode=None [/Script/IOSRuntimeSettings.IOSRuntimeSettings] -MinimumiOSVersion=IOS_8 +MinimumiOSVersion=IOS_11 [/Script/Engine.Engine] @@ -67,4 +67,3 @@ SyncSceneSmoothingFactor=0.000000 AsyncSceneSmoothingFactor=0.990000 InitialAverageFrameRate=0.016667 PhysXTreeRebuildRate=10 - diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 4f61301..d0bc16d 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -124,15 +124,15 @@ 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 (flush_on_draw_count_ == 0) - viewport->GetGameViewport()->IncrementFlushOnDraw(); + //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 (flush_on_draw_count_ > 0) - viewport->GetGameViewport()->DecrementFlushOnDraw(); + //if (flush_on_draw_count_ > 0) + // viewport->GetGameViewport()->DecrementFlushOnDraw(); } } diff --git a/Unreal/Plugins/AirSim/Source/CameraDirector.h b/Unreal/Plugins/AirSim/Source/CameraDirector.h index 8e169ec..32e3fc9 100644 --- a/Unreal/Plugins/AirSim/Source/CameraDirector.h +++ b/Unreal/Plugins/AirSim/Source/CameraDirector.h @@ -13,6 +13,7 @@ UENUM(BlueprintType) enum class ECameraDirectorMode : uint8 { + CAMERA_DIRECTOR_MODE_UNUSED = 0 UMETA(Displayname = "Unused"), CAMERA_DIRECTOR_MODE_FPV = 1 UMETA(DisplayName = "FPV"), CAMERA_DIRECTOR_MODE_GROUND_OBSERVER = 2 UMETA(DisplayName = "GroundObserver"), CAMERA_DIRECTOR_MODE_FLY_WITH_ME = 3 UMETA(DisplayName = "FlyWithMe"), diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index c81cc3d..e8a09c9 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -380,7 +380,9 @@ void PawnSimApi::serviceMoveCameraRequests() AActor* cameraActor = matchedCamera->GetAttachParentActor(); FDetachmentTransformRules detatchRules(EDetachmentRule::KeepWorld, EDetachmentRule::KeepWorld, EDetachmentRule::KeepWorld, true); - matchedCamera->DetachSceneComponentsFromParent(cameraActor->GetRootComponent(), true); + matchedCamera->DetachAllSceneComponents(cameraActor->GetRootComponent(), detatchRules); + matchedCamera->DetachFromActor(detatchRules); + //matchedCamera->DetachSceneComponentsFromParent(cameraActor->GetRootComponent(), true); auto a1 = cameraActor->GetActorLocation(); auto a2 = cameraActor->GetRootComponent()->GetComponentLocation(); matchedCamera->SetActorLocation(cameraActor->GetRootComponent()->GetComponentLocation() + request.transformVec); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 7af6e90..25ca07c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -79,6 +79,8 @@ class AIRSIM_API ASimModeBase : public AActor return static_cast(api_provider_->getVehicleSimApi(vehicle_name)); } + virtual bool isUrdf() { return false; } + protected: //must overrides typedef msr::airlib::AirSimSettings AirSimSettings; diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index 3911797..0d8fd70 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -95,7 +95,7 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const FVector(start.x(), start.y(), start.z()), 5, //size FColor::Blue, - true, //persistent (never goes away) + false, //persistent (never goes away) 0.1 //point leaves a trail on moving object ); } @@ -204,7 +204,7 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms hit_result.ImpactPoint, 5, //size FColor::Red, - true, //persistent (never goes away) + false, //persistent (never goes away) 0.1 //point leaves a trail on moving object ); } @@ -236,7 +236,7 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms endVec, 5, //size FColor::Green, - true, //persistent (never goes away) + false, //persistent (never goes away) 0.1 //point leaves a trail on moving object ); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.cpp index 42b8e30..91bd808 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.cpp @@ -10,7 +10,7 @@ bool StaticMeshGenerator::Initialize(UStaticMesh* boxTemplateMesh, UStaticMesh* return true; } -bool StaticMeshGenerator::CreateUnscaledMeshForLink(FString linkName, UrdfGeometry* visualGeometry, UrdfGeometry* collisionGeometry, APawn* outer, AUrdfLink* link) +bool StaticMeshGenerator::CreateUnscaledMeshForLink(FString linkName, FString linkMaterialName, UrdfGeometry* visualGeometry, UrdfGeometry* collisionGeometry, APawn* outer, AUrdfLink* link, TMap materials) { FName collisionName = FName((linkName + TEXT("_collision")).GetCharArray().GetData()); @@ -58,6 +58,11 @@ bool StaticMeshGenerator::CreateUnscaledMeshForLink(FString linkName, UrdfGeomet meshComponent->SetCollisionEnabled(ECollisionEnabled::Type::QueryAndPhysics); meshComponent->SetNotifyRigidBodyCollision(true); + if (linkMaterialName.Len() > 0) + { + meshComponent->SetMaterial(0, materials[linkMaterialName]); + } + link->SetMeshFromStaticMeshComponent(meshComponent); } else if (geometryType == MESH) @@ -82,6 +87,11 @@ bool StaticMeshGenerator::CreateUnscaledMeshForLink(FString linkName, UrdfGeomet meshComponent->SetCollisionEnabled(ECollisionEnabled::Type::QueryAndPhysics); meshComponent->SetNotifyRigidBodyCollision(true); + if (linkMaterialName.Len() > 0) + { + meshComponent->SetMaterial(0, materials[linkMaterialName]); + } + link->SetMeshFromStaticMeshComponent(meshComponent); } else @@ -182,17 +192,32 @@ TArray> StaticMeshGenerator::CreateCollisionVAHCD(TArrayCompute(verts, 3, numVerts, tris, 3, numTris, VHACD_Params); + + bool success = vhacdInterface->Compute(verts, numVerts, tris, numTris, VHACD_Params); + //bool success = vhacdInterface->Compute(verts, 3, numVerts, tris, 3, numTris, VHACD_Params); if (!success) { vhacdInterface->Clean(); vhacdInterface->Release(); + delete verts; return TArray>(); } @@ -221,6 +246,7 @@ TArray> StaticMeshGenerator::CreateCollisionVAHCD(TArrayClean(); vhacdInterface->Release(); + delete verts; return output; } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.h b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.h index 014f534..8329de8 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/MeshGeneration/StaticMeshGenerator.h @@ -30,7 +30,7 @@ class StaticMeshGenerator { public: bool Initialize(UStaticMesh* boxTemplateMesh, UStaticMesh* cylinderTemplateMesh, UStaticMesh* sphereTemplateMesh, TMap unrealMeshes); - bool CreateUnscaledMeshForLink(FString linkName, UrdfGeometry* visualGeometry, UrdfGeometry* collisionGeometry, APawn* outer, AUrdfLink* link); + bool CreateUnscaledMeshForLink(FString linkName, FString linkMaterialName, UrdfGeometry* visualGeometry, UrdfGeometry* collisionGeometry, APawn* outer, AUrdfLink* link, TMap materials); private: void ParseProceduralMeshSpecification(FString fileName, ProceduralMeshFileType fileType, bool reverseNormals, float scaleFactor, ProceduralMeshSpecification& meshSpecification); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/SimModeUrdfBot.h b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/SimModeUrdfBot.h index 0432741..44844d6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/SimModeUrdfBot.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/SimModeUrdfBot.h @@ -27,11 +27,13 @@ class AIRSIM_API ASimModeUrdfBot : public ASimModeBase virtual void pause(bool is_paused) override; virtual void continueForTime(double seconds) override; + virtual bool isUrdf() override { return true; } + protected: virtual void setupClockSpeed() override; virtual std::unique_ptr createApiServer() const override; virtual void setupVehiclesAndCamera() override; - virtual void ASimModeUrdfBot::getExistingVehiclePawns(TArray& pawns) const override; + virtual void getExistingVehiclePawns(TArray& pawns) const override; private: diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.cpp index 1e08d7a..8750fdc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.cpp @@ -55,11 +55,11 @@ AUrdfBotPawn::AUrdfBotPawn() FString materialPath = kvp.Value->TextureFile; if (materialPath.Len() > 0) { - ConstructorHelpers::FObjectFinder material(*materialPath); + ConstructorHelpers::FObjectFinder material(*materialPath); if (material.Object != NULL) { - this->materials_.Add(materialName, static_cast(material.Object)); + this->materials_.Add(materialName, static_cast(material.Object)); } } } @@ -307,7 +307,9 @@ void AUrdfBotPawn::ConstructFromFile(FString fileName) UrdfLinkSpecification* rootLinkSpecification = this->FindRootNodeSpecification(links); this->root_component_ = this->components_[rootLinkSpecification->Name]; this->root_component_->SetReferenceFrameLocation(this->GetActorLocation(), this->GetActorRotation()); - this->root_component_->GetRootComponent()->AttachTo(RootComponent, NAME_None, EAttachLocation::KeepRelativeOffset); //um... + this->root_component_->GetRootComponent()->AttachTo(RootComponent, NAME_None, EAttachLocation::KeepRelativeOffset); + + this->SetRootComponent(this->root_component_->GetRootComponent()); UPrimitiveComponent* rootCollisionComponent = this->root_component_->GetCollisionComponent(); if (rootCollisionComponent != nullptr) @@ -360,18 +362,12 @@ AUrdfLink* AUrdfBotPawn::CreateLinkFromSpecification(const UrdfLinkSpecification AUrdfLink* link = NewObject(this, AUrdfLink::StaticClass(), FName(linkSpecification.Name.GetCharArray().GetData())); - this->staticMeshGenerator_.CreateUnscaledMeshForLink(linkSpecification.Name, visualGeometry, collisionGeometry, this, link); + this->staticMeshGenerator_.CreateUnscaledMeshForLink(linkSpecification.Name, linkSpecification.VisualSpecification->MaterialName, visualGeometry, collisionGeometry, this, link, this->materials_); this->ResizeLink(link, collisionGeometry); - if (linkSpecification.VisualSpecification->MaterialName.Len() > 0) - { - link->SetMaterial(this->materials_[linkSpecification.VisualSpecification->MaterialName]); - } - link->SetMass(linkSpecification.InertialSpecification->Mass); link->SetOwningActor(this); - auto ww = link->GetName(); return link; } @@ -417,11 +413,6 @@ void AUrdfBotPawn::AttachChildren(AUrdfLink* parentLink, const UrdfLinkSpecifica FRotator rotation = FRotator::ZeroRotator; if (jointSpecification->Type != FIXED_TYPE) { - if (!jointSpecification->Axis.Normalize()) - { - int j = 0; - } - // Rotate such that the local X axis aligns with the specified axis FVector unitX(1, 0, 0); FVector unitY(0, 1, 0); @@ -506,6 +497,7 @@ FConstraintInstance AUrdfBotPawn::CreateConstraint(const UrdfJointSpecification constraintInstance.ProfileInstance.ConeLimit.bSoftConstraint = false; constraintInstance.ProfileInstance.AngularDrive.AngularDriveMode = EAngularDriveMode::TwistAndSwing; + float range = 0.0f; switch (jointSpecification.Type) { @@ -691,7 +683,6 @@ FVector AUrdfBotPawn::MoveChildLinkForLimitedXAxisMotion(AUrdfLink* parentLink, inverseTransform = -1 * transform; childLink->SetActorLocation(childLocation + transform, false, nullptr, ETeleportType::TeleportPhysics); - FVector ww = childLink->GetActorLocation(); return inverseTransform; } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.h index 7efb838..05d3efc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfBotPawn.h @@ -121,7 +121,7 @@ class AUrdfBotPawn : public APawn, public AirsimVehicle USceneComponent* rootComponent_; UPROPERTY() - TMap materials_; + TMap materials_; UPROPERTY() TMap user_static_meshes_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfLink.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfLink.cpp index af4f090..4ff9b99 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfLink.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfLink.cpp @@ -51,8 +51,6 @@ void AUrdfLink::Tick(float delta) this->UpdateKinematics(delta); } - - void AUrdfLink::TickPostPhysics(float deltaSeconds, ELevelTick tickType, FAUrdfLinkSecondaryTickFunction& thisTickFunction) { } @@ -280,7 +278,7 @@ void AUrdfLink::InitPostSetMesh() // The default Position and Velocity iterations are set very low. // This can cause "bouncy" joints that should be stiff, and in extreme cases can cause joints to settle in the wrong place. // 10000 is an arbitrary number that appears to work well without using too much CPU. - int32 cnt = 10000; + int32 cnt = 100000; this->mesh_root_->GetBodyInstance()->PositionSolverIterationCount = cnt; this->mesh_root_->GetBodyInstance()->VelocitySolverIterationCount = cnt; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfParser/UrdfJointSpecification.h b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfParser/UrdfJointSpecification.h index 13d0e71..0aec3f7 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfParser/UrdfJointSpecification.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/UrdfBot/UrdfParser/UrdfJointSpecification.h @@ -78,7 +78,7 @@ class UrdfJointSpecification UrdfJointMimicSpecification* Mimic = nullptr; UrdfJointSafetyControllerSpecification* SafetyController = nullptr; - static UrdfJointType UrdfJointSpecification::ParseJointType(FString type) + static UrdfJointType ParseJointType(FString type) { if (type.Equals(TEXT("revolute"))) return REVOLUTE_TYPE; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 33b971a..2588589 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -62,15 +62,37 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con Pose result; UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); - result = actor ? simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation())) - : Pose::nanPose(); + + if (!actor) + { + result = Pose::nanPose(); + } + else + { + if (simmode_->isUrdf()) + { + NedTransform transform = simmode_->getGlobalNedTransform(); + + FQuat rotation = actor->GetActorQuat(); + FVector location = actor->GetActorLocation(); + + result = Pose( + Vector3r(transform.toNed(location.X), transform.toNed(location.Y), transform.toNed(location.Z)), + Quaternionr(rotation.W, rotation.X, rotation.Y, rotation.Z) + ); + } + else + { + result = simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation())); + } + } }, true); return result; } bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimApi::Pose& pose, bool teleport) { - bool result; + bool result = false; UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &pose, teleport, &result]() { FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose); AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); @@ -86,6 +108,59 @@ bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimAp return result; } +bool WorldSimApi::spawnStaticMeshObject(const std::string& mesh_path, const std::string& object_name, const WorldSimApi::Pose &pose) +{ + bool result = false; + UAirBlueprintLib::RunCommandOnGameThread([this, &mesh_path, &object_name, &pose, &result]() { + //UClass* class_instance = UAirBlueprintLib::LoadClass(mesh_path); + + // Use raw unreal coordinates + // But take into account the scale. The user will provide offsets in meters. + // The function fromNed will convert from meters to UU. + NedTransform transform = simmode_->getGlobalNedTransform(); + + FTransform spawn_transform(FQuat(pose.orientation.x(), + pose.orientation.y(), + pose.orientation.z(), + pose.orientation.w()), + FVector(transform.fromNed(pose.position.x()), + transform.fromNed(pose.position.y()), + transform.fromNed(pose.position.z()))); + + FActorSpawnParameters spawnParameters; + spawnParameters.Name = FName(object_name.c_str()); + + AStaticMeshActor* spawned_actor = this->simmode_->GetWorld()->SpawnActor(AStaticMeshActor::StaticClass(), spawn_transform, spawnParameters); + UStaticMesh* mesh = LoadObject(spawned_actor, UTF8_TO_TCHAR(mesh_path.c_str())); + + spawned_actor->SetMobility(EComponentMobility::Movable); + spawned_actor->GetStaticMeshComponent()->SetStaticMesh(mesh); + spawned_actor->GetStaticMeshComponent()->SetSimulatePhysics(true); + spawned_actor->GetStaticMeshComponent()->SetCollisionObjectType(ECollisionChannel::ECC_PhysicsBody); + spawned_actor->GetStaticMeshComponent()->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Block); + spawned_actor->GetStaticMeshComponent()->SetCollisionEnabled(ECollisionEnabled::Type::QueryAndPhysics); + spawned_actor->GetStaticMeshComponent()->SetNotifyRigidBodyCollision(true); + + result = true; + }, true); + return result; +} + +bool WorldSimApi::deleteObject(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 != nullptr) + { + this->simmode_->GetWorld()->DestroyActor(actor); + result = true; + } + + }, true); + return result; +} + void WorldSimApi::charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) { AAirSimCharacter* character = getAirSimCharacter(character_name); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index fe22b0e..2b981a3 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -6,6 +6,8 @@ #include "SimMode/SimModeBase.h" #include "AirSimCharacter.h" #include +#include "Engine/StaticMeshActor.h" +#include "common/Common.hpp" class WorldSimApi : public msr::airlib::WorldSimApiBase { public: @@ -27,6 +29,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { 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 bool spawnStaticMeshObject(const std::string& mesh_path, const std::string& object_name, const Pose& pose) override; + virtual bool deleteObject(const std::string& object_name) override; //----------- APIs to control ACharacter in scene ----------/ virtual void charSetFaceExpression(const std::string& expression_name, float value, const std::string& character_name) override;