Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ability to spawn/destroy lights and control light parameters #3991

Merged
merged 10 commits into from
Nov 11, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions AirLib/include/api/WorldSimApiBase.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ namespace airlib

// ------ 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, bool physics_enabled) = 0;
virtual bool destroyObject(const string& object_name) = 0;
virtual string spawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled, bool is_blueprint) = 0;
virtual bool destroyObject(const std::string& object_name) = 0;

virtual bool isPaused() const = 0;
virtual void reset() = 0;
Expand Down Expand Up @@ -72,6 +72,7 @@ namespace airlib
virtual bool runConsoleCommand(const std::string& command) = 0;
virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0;
virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0;
virtual bool setLightIntensity(const std::string& light_name, float intensity) = 0;
virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) = 0;
virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) = 0;
virtual vector<MeshPositionVertexBuffersResponse> getMeshPositionVertexBuffers() const = 0;
Expand Down
9 changes: 7 additions & 2 deletions AirLib/src/api/RpcLibServerBase.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -302,8 +302,8 @@ namespace airlib
return getWorldSimApi()->loadLevel(level_name);
});

pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdaptorsBase::Pose& pose, const RpcLibAdaptorsBase::Vector3r& scale, bool physics_enabled) -> string {
return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to(), physics_enabled);
pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdaptorsBase::Pose& pose, const RpcLibAdaptorsBase::Vector3r& scale, bool physics_enabled, bool is_blueprint) -> string {
return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to(), physics_enabled, is_blueprint);
});

pimpl_->server.bind("simDestroyObject", [&](const string& object_name) -> bool {
Expand Down Expand Up @@ -389,10 +389,15 @@ namespace airlib
const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState();
return RpcLibAdaptorsBase::EnvironmentState(result);
});

pimpl_->server.bind("simCreateVoxelGrid", [&](const RpcLibAdaptorsBase::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) -> bool {
return getWorldSimApi()->createVoxelGrid(position.to(), x, y, z, res, output_file);
});

pimpl_->server.bind("simSetLightIntensity", [&](const std::string& light_name, float intensity) -> bool {
return getWorldSimApi()->setLightIntensity(light_name, intensity);
});

pimpl_->server.bind("cancelLastTask", [&](const std::string& vehicle_name) -> void {
getVehicleApi(vehicle_name)->cancelLastTask();
});
Expand Down
19 changes: 17 additions & 2 deletions PythonClient/airsim/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,19 @@ def confirmConnection(self):
print(ver_info)
print('')

def simSetLightIntensity(self, light_name, intensity):
"""
Change intensity of named light

Args:
light_name (str): Name of light to change
intensity (float): New intensity value
zimmy87 marked this conversation as resolved.
Show resolved Hide resolved

Returns:
bool: True if successful, otherwise False
"""
return self.client.call("simSetLightIntensity", light_name, intensity)

def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0):
"""
Runtime Swap Texture API
Expand Down Expand Up @@ -483,19 +496,21 @@ def simLoadLevel(self, level_name):
"""
return self.client.call('simLoadLevel', level_name)

def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=False):
def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=False, is_blueprint=False):
"""Spawned selected object in the world

Args:
object_name (str): Desired name of new object
asset_name (str): Name of asset(mesh) in the project database
pose (airsim.Pose): Desired pose of object
scale (airsim.Vector3r): Desired scale of object
physics_enabled (bool, optional): Whether to enable physics for the object
is_blueprint (bool, optional): Whether to spawn a blueprint or an actor

Returns:
str: Name of spawned object, in case it had to be modified
"""
return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled)
return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled, is_blueprint)

def simDestroyObject(self, object_name):
"""Removes selected object from the world
Expand Down
23 changes: 23 additions & 0 deletions PythonClient/environment/light_control.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
import airsim
import time

client = airsim.VehicleClient()
client.confirmConnection()

# Access an existing light in the world
lights = client.simListSceneObjects("PointLight.*")
pose = client.simGetObjectPose(lights[0])
scale = airsim.Vector3r(1, 1, 1)

# Destroy the light
client.simDestroyObject(lights[0])
time.sleep(1)

# Create a new light at the same pose
new_light_name = client.simSpawnObject("PointLight", "PointLightBP", pose, scale, False, True)
time.sleep(1)

# Change the light's intensity
for i in range(20):
client.simSetLightIntensity(new_light_name, i * 100)
time.sleep(0.5)
8 changes: 8 additions & 0 deletions Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,14 @@ void WorldSimApi::printLogMessage(const std::string& message, const std::string&
PrintLogMessage(message.c_str(), message_param.c_str(), "", severity);
}

bool WorldSimApi::setLightIntensity(const std::string& light_name, float intensity)
{
throw std::invalid_argument(common_utils::Utils::stringf(
"setLightIntensity is not supported on unity")
.c_str());
return false;
}

std::unique_ptr<std::vector<std::string>> WorldSimApi::swapTextures(const std::string& tag, int tex_id, int component_id, int material_id)
{
std::unique_ptr<std::vector<std::string>> result;
Expand Down
3 changes: 2 additions & 1 deletion Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase

// ------ Level setting apis ----- //
virtual bool loadLevel(const std::string& level_name) override { return false; };
virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) override { return ""; };
virtual std::string spawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled, bool is_blueprint) override { return ""; };
virtual bool destroyObject(const std::string& object_name) override { return false; };

virtual bool isPaused() const override;
Expand All @@ -37,6 +37,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
virtual void printLogMessage(const std::string& message,
const std::string& message_param = "", unsigned char severity = 0) override;

virtual bool setLightIntensity(const std::string& light_name, float intensity) override;
virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override;
virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) override;
virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) override;
Expand Down
Binary file not shown.
Binary file not shown.
1 change: 1 addition & 0 deletions Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,7 @@ void UAirBlueprintLib::GenerateAssetRegistryMap(const UObject* context, TMap<FSt
UAirBlueprintLib::RunCommandOnGameThread([context, &asset_map]() {
FARFilter Filter;
Filter.ClassNames.Add(UStaticMesh::StaticClass()->GetFName());
Filter.ClassNames.Add(UBlueprint::StaticClass()->GetFName());
Filter.bRecursivePaths = true;

auto world = context->GetWorld();
Expand Down
107 changes: 70 additions & 37 deletions Unreal/Plugins/AirSim/Source/WorldSimApi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,11 @@
#include "DrawDebugHelpers.h"
#include "Runtime/Engine/Classes/Components/LineBatchComponent.h"
#include "Runtime/Engine/Classes/Engine/Engine.h"
#include "Misc/OutputDeviceNull.h"
#include "ImageUtils.h"
#include <cstdlib>
#include <ctime>
#include <algorithm>

WorldSimApi::WorldSimApi(ASimModeBase* simmode)
: simmode_(simmode) {}
Expand Down Expand Up @@ -83,65 +85,69 @@ bool WorldSimApi::destroyObject(const std::string& object_name)
return result;
}

std::string WorldSimApi::spawnObject(std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled)
std::string WorldSimApi::spawnObject(const std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled, bool is_blueprint)
{
FString asset_name(load_object.c_str());
FAssetData* load_asset = simmode_->asset_map.Find(asset_name);

if (!load_asset->IsValid()) {
throw std::invalid_argument("There were no objects with name " + load_object + " found in the Registry");
}

// Create struct for Location and Rotation of actor in Unreal
FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose);

bool found_object = false, spawned_object = false;
UAirBlueprintLib::RunCommandOnGameThread([this, load_object, &object_name, &actor_transform, &found_object, &spawned_object, &scale, &physics_enabled]() {
FString asset_name = FString(load_object.c_str());
FAssetData* LoadAsset = simmode_->asset_map.Find(asset_name);

if (LoadAsset) {
found_object = true;
UStaticMesh* LoadObject = dynamic_cast<UStaticMesh*>(LoadAsset->GetAsset());
std::vector<std::string> 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;
}
bool spawned_object = false;
std::string final_object_name = object_name;

UAirBlueprintLib::RunCommandOnGameThread([this, load_asset, &final_object_name, &spawned_object, &actor_transform, &scale, &physics_enabled, &is_blueprint]() {
// Ensure new non-matching name for the object
std::vector<std::string> matching_names = UAirBlueprintLib::ListMatchingActors(simmode_, ".*" + final_object_name + ".*");
if (matching_names.size() > 0) {
int greatest_num{ 0 };
for (const auto& match : matching_names) {
std::string number_extension = match.substr(match.find_last_not_of("0123456789") + 1);
if (number_extension != "") {
greatest_num = std::max(greatest_num, std::stoi(number_extension));
}
object_name += std::to_string(greatest_num + 1);
}
FActorSpawnParameters new_actor_spawn_params;
new_actor_spawn_params.Name = FName(object_name.c_str());
//new_actor_spawn_params.NameMode = FActorSpawnParameters::ESpawnActorNameMode::Required_ReturnNull;
AActor* NewActor = this->createNewActor(new_actor_spawn_params, actor_transform, scale, LoadObject);

if (NewActor) {
spawned_object = true;
simmode_->scene_object_map.Add(FString(object_name.c_str()), NewActor);
}
final_object_name += std::to_string(greatest_num + 1);
}

FActorSpawnParameters new_actor_spawn_params;
new_actor_spawn_params.Name = FName(final_object_name.c_str());

UAirBlueprintLib::setSimulatePhysics(NewActor, physics_enabled);
AActor* NewActor;
if (is_blueprint) {
UBlueprint* LoadObject = Cast<UBlueprint>(load_asset->GetAsset());
NewActor = this->createNewBPActor(new_actor_spawn_params, actor_transform, scale, LoadObject);
}
else {
found_object = false;
UStaticMesh* LoadObject = dynamic_cast<UStaticMesh*>(load_asset->GetAsset());
NewActor = this->createNewStaticMeshActor(new_actor_spawn_params, actor_transform, scale, LoadObject);
}

if (IsValid(NewActor)) {
spawned_object = true;
simmode_->scene_object_map.Add(FString(final_object_name.c_str()), NewActor);
}

UAirBlueprintLib::setSimulatePhysics(NewActor, physics_enabled);
},
true);

if (!found_object) {
throw std::invalid_argument(
"There were no objects with name " + load_object + " found in the Registry");
}
if (!spawned_object) {
throw std::invalid_argument(
"Engine could not spawn " + load_object + " because of a stale reference of same name");
}
return object_name;
return final_object_name;
}

AActor* WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh)
AActor* WorldSimApi::createNewStaticMeshActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh)
{
AActor* NewActor = simmode_->GetWorld()->SpawnActor<AActor>(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, spawn_params);

if (NewActor) {
if (IsValid(NewActor)) {
UStaticMeshComponent* ObjectComponent = NewObject<UStaticMeshComponent>(NewActor);
ObjectComponent->SetStaticMesh(static_mesh);
ObjectComponent->SetRelativeLocation(FVector(0, 0, 0));
Expand All @@ -154,6 +160,33 @@ AActor* WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, c
return NewActor;
}

AActor* WorldSimApi::createNewBPActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UBlueprint* blueprint)
{
UClass* new_bp = static_cast<UClass*>(blueprint->GeneratedClass);
AActor* new_actor = simmode_->GetWorld()->SpawnActor<AActor>(new_bp, FVector::ZeroVector, FRotator::ZeroRotator, spawn_params);

if (new_actor) {
new_actor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics);
}
return new_actor;
}

bool WorldSimApi::setLightIntensity(const std::string& light_name, float intensity)
{
bool result = false;
UAirBlueprintLib::RunCommandOnGameThread([this, &light_name, &intensity, &result]() {
AActor* light_actor = simmode_->scene_object_map.FindRef(FString(light_name.c_str()));

if (light_actor) {
const FString command = FString::Printf(TEXT("SetIntensity %f"), intensity);
FOutputDeviceNull ar;
result = light_actor->CallFunctionByNameWithArguments(*command, ar, nullptr, true);
}
},
true);
return result;
}
saihv marked this conversation as resolved.
Show resolved Hide resolved

bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file)
{
bool success = false;
Expand Down
6 changes: 4 additions & 2 deletions Unreal/Plugins/AirSim/Source/WorldSimApi.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase

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, bool physics_enabled) override;
virtual std::string spawnObject(const std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled, bool is_blueprint) override;
virtual bool destroyObject(const std::string& object_name) override;

virtual bool isPaused() const override;
Expand All @@ -47,6 +47,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
virtual void printLogMessage(const std::string& message,
const std::string& message_param = "", unsigned char severity = 0) override;

virtual bool setLightIntensity(const std::string& light_name, float intensity) override;
virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override;
virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) override;
virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) override;
Expand Down Expand Up @@ -99,7 +100,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
virtual std::vector<msr::airlib::DetectionInfo> getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override;

private:
AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh);
AActor* createNewStaticMeshActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh);
AActor* createNewBPActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UBlueprint* blueprint);
void spawnPlayer();

private:
Expand Down