From 6be021212c4f05044ee4f877511b0b2544e5ce0a Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Wed, 9 Dec 2020 21:24:02 -0800 Subject: [PATCH 1/9] Voxel grid construction --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/WorldSimApiBase.hpp | 2 + AirLib/src/api/RpcLibClientBase.cpp | 5 ++ AirLib/src/api/RpcLibServerBase.cpp | 3 + PythonClient/airsim/client.py | 15 +++- Unreal/Environments/Blocks/Blocks.uproject | 16 ++--- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 73 ++++++++++++++++++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 3 +- 8 files changed, 108 insertions(+), 11 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index b14e18f5d1..81b572b906 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -107,7 +107,7 @@ class RpcLibClientBase { void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = ""); // This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = ""); - + void simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 286b98d99d..f179b8222d 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -67,6 +67,8 @@ class WorldSimApiBase { 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; + virtual void createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) = 0; + // Recording APIs virtual void startRecording() = 0; virtual void stopRecording() = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 04b6e33ce1..a3d38c13f4 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -406,6 +406,11 @@ msr::airlib::Environment::State RpcLibClientBase::simGetGroundTruthEnvironment(c { return pimpl_->client.call("simGetGroundTruthEnvironment", vehicle_name).as().to();; } +void RpcLibClientBase::simCreateVoxelGrid(const msr::airlib::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) +{ + pimpl_->client.call("simCreateVoxelGrid", RpcLibAdapatorsBase::Vector3r(position), x, y, z, res, output_file); +} + void RpcLibClientBase::cancelLastTask(const std::string& vehicle_name) { diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 52baf8666f..df52bf7bd8 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -344,6 +344,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState(); return RpcLibAdapatorsBase::EnvironmentState(result); }); + pimpl_->server.bind("simCreateVoxelGrid", [&](const RpcLibAdapatorsBase::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) -> void { + return getWorldSimApi()->createVoxelGrid(position.to(), x, y, z, res, output_file); + }); pimpl_->server.bind("cancelLastTask", [&](const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->cancelLastTask(); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index ca8abc481b..723d58f809 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -790,6 +790,19 @@ def simSetWind(self, wind): """ self.client.call('simSetWind', wind) + def createVoxelGrid(self, position, x, y, z, res, of): + """ + Construct and save a binvox-formatted voxel grid of environment + + Args: + position (Vector3r): Position around which voxel grid is centered in m + x, y, z (float): Size of each voxel grid dimension in m + res (float): Resolution of voxel grid in m + of (str): Name of output file to save voxel grid as + + """ + self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) + # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): @@ -1255,4 +1268,4 @@ def getCarControls(self, vehicle_name=''): CarControls: """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) + return CarControls.from_msgpack(controls_raw) \ No newline at end of file diff --git a/Unreal/Environments/Blocks/Blocks.uproject b/Unreal/Environments/Blocks/Blocks.uproject index 76c9ca2c23..1f6424fd2b 100644 --- a/Unreal/Environments/Blocks/Blocks.uproject +++ b/Unreal/Environments/Blocks/Blocks.uproject @@ -19,12 +19,12 @@ "Enabled": true }, { - "Name": "SteamVR", - "Enabled": false - }, - { - "Name": "OculusVR", - "Enabled": false - }, + "Name": "SteamVR", + "Enabled": false + }, + { + "Name": "OculusVR", + "Enabled": false + } ] -} +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 61fe335bfa..3e08eb6995 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -161,6 +161,79 @@ AActor* WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, c return NewActor; } +void 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) +{ + int ncells_x = x_size / res; + int ncells_y = y_size / res; + int ncells_z = z_size / res; + + voxel_grid_.resize(ncells_x * ncells_y * ncells_z); + + int ctr = 0; + float scale = 1 / res; + float scale_cm = scale * 100; + FCollisionQueryParams params; + params.bFindInitialOverlaps = true; + params.bTraceComplex = false; + params.TraceTag = ""; + auto position_in_UE_frame = simmode_->getGlobalNedTransform().fromGlobalNed(position); + for (float i = 0; i < ncells_x; i++) { + for (float k = 0; k < ncells_z; k++) { + for (float j = 0; j < ncells_y; j++) { + int idx = i + ncells_x * (k + ncells_z * j); + FVector position = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame; + voxel_grid_[idx] = (unsigned int)simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(res/50)), params); + + } + } + } + + std::ofstream* output = new std::ofstream(output_file, std::ios::out | std::ios::binary); + if (!output->good()) + { + std::cerr << "Error: Could not open output file " << output << "!" << std::endl; + exit(1); + } + + // Write the binvox file using run-length encoding + // where each pair of bytes is of the format (run value, run length) + *output << "#binvox 1\n"; + *output << "dim " << ncells_x << " " << ncells_z << " " << ncells_y << "\n"; + *output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << int(-z_size / 2) << "\n"; + *output << "scale " << scale << "\n"; + *output << "data\n"; + bool run_value = voxel_grid_[0]; + unsigned int run_length = 0; + for (size_t i = 0; i < voxel_grid_.size(); ++i) + { + if (voxel_grid_[i] == run_value) + { + // This is a run (repeated bit value) + run_length++; + if (run_length == 255) + { + *output << static_cast(run_value); + *output << static_cast(run_length); + run_length = 0; + } + } + else + { + // End of a run + *output << static_cast(run_value); + *output << static_cast(run_length); + run_value = voxel_grid_[i]; + run_length = 1; + } + } + if (run_length > 0) + { + *output << static_cast(run_value); + *output << static_cast(run_length); + } + output->close(); +} + bool WorldSimApi::isPaused() const { return simmode_->isPaused(); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index f83353b5b2..0276c615be 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -65,6 +65,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool isRecording() const override; virtual void setWind(const Vector3r& wind) const override; + virtual void createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); @@ -73,4 +74,4 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { private: ASimModeBase* simmode_; ULevelStreamingDynamic* current_level_; -}; +}; \ No newline at end of file From d8218c8742254afc0ddca444b6b21f23b6c252f8 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Fri, 11 Dec 2020 18:50:39 -0800 Subject: [PATCH 2/9] Fix box scaling --- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 3e08eb6995..8d51e120ef 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -170,7 +170,7 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c voxel_grid_.resize(ncells_x * ncells_y * ncells_z); int ctr = 0; - float scale = 1 / res; + float scale = res; float scale_cm = scale * 100; FCollisionQueryParams params; params.bFindInitialOverlaps = true; @@ -182,7 +182,7 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c for (float j = 0; j < ncells_y; j++) { int idx = i + ncells_x * (k + ncells_z * j); FVector position = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame; - voxel_grid_[idx] = (unsigned int)simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(res/50)), params); + voxel_grid_[idx] = (unsigned int)simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); } } @@ -199,8 +199,8 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c // where each pair of bytes is of the format (run value, run length) *output << "#binvox 1\n"; *output << "dim " << ncells_x << " " << ncells_z << " " << ncells_y << "\n"; - *output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << int(-z_size / 2) << "\n"; - *output << "scale " << scale << "\n"; + *output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << 0 << "\n"; + *output << "scale " << 1/x_size << "\n"; *output << "data\n"; bool run_value = voxel_grid_[0]; unsigned int run_length = 0; From 6276c89bfd16eafe0541dbd9f2022501e6e2e2af Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Fri, 11 Dec 2020 23:59:09 -0800 Subject: [PATCH 3/9] Add missing decl, fixes --- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 36 +++++++++----------- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 1 + 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 8d51e120ef..d2903ad0cc 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -170,8 +170,7 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c voxel_grid_.resize(ncells_x * ncells_y * ncells_z); int ctr = 0; - float scale = res; - float scale_cm = scale * 100; + float scale_cm = res * 100; FCollisionQueryParams params; params.bFindInitialOverlaps = true; params.bTraceComplex = false; @@ -182,26 +181,25 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c for (float j = 0; j < ncells_y; j++) { int idx = i + ncells_x * (k + ncells_z * j); FVector position = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame; - voxel_grid_[idx] = (unsigned int)simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); - + voxel_grid_[idx] = simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); } } } - std::ofstream* output = new std::ofstream(output_file, std::ios::out | std::ios::binary); - if (!output->good()) + std::ofstream output(output_file, std::ios::out | std::ios::binary); + if (!output.good()) { - std::cerr << "Error: Could not open output file " << output << "!" << std::endl; + std::cerr << "Error: Could not open output file " << &output << "!" << std::endl; exit(1); } // Write the binvox file using run-length encoding // where each pair of bytes is of the format (run value, run length) - *output << "#binvox 1\n"; - *output << "dim " << ncells_x << " " << ncells_z << " " << ncells_y << "\n"; - *output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << 0 << "\n"; - *output << "scale " << 1/x_size << "\n"; - *output << "data\n"; + output << "#binvox 1\n"; + output << "dim " << ncells_x << " " << ncells_z << " " << ncells_y << "\n"; + output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << int(-z_size/2) << "\n"; + output << "scale " << 1/x_size << "\n"; + output << "data\n"; bool run_value = voxel_grid_[0]; unsigned int run_length = 0; for (size_t i = 0; i < voxel_grid_.size(); ++i) @@ -212,26 +210,26 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c run_length++; if (run_length == 255) { - *output << static_cast(run_value); - *output << static_cast(run_length); + output << static_cast(run_value); + output << static_cast(run_length); run_length = 0; } } else { // End of a run - *output << static_cast(run_value); - *output << static_cast(run_length); + output << static_cast(run_value); + output << static_cast(run_length); run_value = voxel_grid_[i]; run_length = 1; } } if (run_length > 0) { - *output << static_cast(run_value); - *output << static_cast(run_length); + output << static_cast(run_value); + output << static_cast(run_length); } - output->close(); + output.close(); } bool WorldSimApi::isPaused() const diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 0276c615be..daf717a420 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -74,4 +74,5 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { private: ASimModeBase* simmode_; ULevelStreamingDynamic* current_level_; + std::vector voxel_grid_; }; \ No newline at end of file From 1d415aae7310575f4291f5dc283ef06578e11fc5 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 13:38:20 -0800 Subject: [PATCH 4/9] Return bool status, print UE error if failed --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 4 ++-- PythonClient/airsim/client.py | 2 +- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 9 ++++++--- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 6 files changed, 12 insertions(+), 9 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 81b572b906..bb8692e52b 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -107,7 +107,7 @@ class RpcLibClientBase { void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = ""); // This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = ""); - void simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); + bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index f179b8222d..cf27afbe1a 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -67,7 +67,7 @@ class WorldSimApiBase { 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; - virtual void createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) = 0; + virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) = 0; // Recording APIs virtual void startRecording() = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index a3d38c13f4..190af53a24 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -406,9 +406,9 @@ msr::airlib::Environment::State RpcLibClientBase::simGetGroundTruthEnvironment(c { return pimpl_->client.call("simGetGroundTruthEnvironment", vehicle_name).as().to();; } -void RpcLibClientBase::simCreateVoxelGrid(const msr::airlib::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) +bool RpcLibClientBase::simCreateVoxelGrid(const msr::airlib::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) { - pimpl_->client.call("simCreateVoxelGrid", RpcLibAdapatorsBase::Vector3r(position), x, y, z, res, output_file); + return pimpl_->client.call("simCreateVoxelGrid", RpcLibAdapatorsBase::Vector3r(position), x, y, z, res, output_file); } diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 723d58f809..728bc20274 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -801,7 +801,7 @@ def createVoxelGrid(self, position, x, y, z, res, of): of (str): Name of output file to save voxel grid as """ - self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) + return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index d2903ad0cc..c288194513 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -161,8 +161,9 @@ AActor* WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, c return NewActor; } -void 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 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; int ncells_x = x_size / res; int ncells_y = y_size / res; int ncells_z = z_size / res; @@ -189,8 +190,8 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c std::ofstream output(output_file, std::ios::out | std::ios::binary); if (!output.good()) { - std::cerr << "Error: Could not open output file " << &output << "!" << std::endl; - exit(1); + UE_LOG(LogTemp, Error, TEXT("Could not open output file for voxel grid!")); + return success; } // Write the binvox file using run-length encoding @@ -230,6 +231,8 @@ void WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c output << static_cast(run_length); } output.close(); + success = true; + return success; } bool WorldSimApi::isPaused() const diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index daf717a420..2a96852343 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -65,7 +65,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual bool isRecording() const override; virtual void setWind(const Vector3r& wind) const override; - virtual void createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; + virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; private: AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); From 24171d6254821936758999ae77fecb51b02a49a3 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 14:07:10 -0800 Subject: [PATCH 5/9] Fix rpc return type --- AirLib/src/api/RpcLibClientBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 190af53a24..18b1c18015 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -408,7 +408,7 @@ msr::airlib::Environment::State RpcLibClientBase::simGetGroundTruthEnvironment(c } bool RpcLibClientBase::simCreateVoxelGrid(const msr::airlib::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) { - return pimpl_->client.call("simCreateVoxelGrid", RpcLibAdapatorsBase::Vector3r(position), x, y, z, res, output_file); + return pimpl_->client.call("simCreateVoxelGrid", RpcLibAdapatorsBase::Vector3r(position), x, y, z, res, output_file).as(); } From 85f612015abf860f04eb2ac052c4ba8682e3654c Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 16:16:07 -0800 Subject: [PATCH 6/9] Fix rpc return type --- AirLib/src/api/RpcLibServerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index df52bf7bd8..80e2c47af5 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -344,7 +344,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState(); return RpcLibAdapatorsBase::EnvironmentState(result); }); - pimpl_->server.bind("simCreateVoxelGrid", [&](const RpcLibAdapatorsBase::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) -> void { + pimpl_->server.bind("simCreateVoxelGrid", [&](const RpcLibAdapatorsBase::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); }); From bcd9413ff258a569a8283623c8a8d743d60a1fb2 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 23:50:40 -0800 Subject: [PATCH 7/9] Minor fixes --- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index c288194513..935782dd4c 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -170,7 +170,6 @@ bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c voxel_grid_.resize(ncells_x * ncells_y * ncells_z); - int ctr = 0; float scale_cm = res * 100; FCollisionQueryParams params; params.bFindInitialOverlaps = true; @@ -190,7 +189,7 @@ bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c std::ofstream output(output_file, std::ios::out | std::ios::binary); if (!output.good()) { - UE_LOG(LogTemp, Error, TEXT("Could not open output file for voxel grid!")); + UE_LOG(LogTemp, Error, TEXT("Could not open output file to write voxel grid!")); return success; } @@ -198,8 +197,8 @@ bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c // where each pair of bytes is of the format (run value, run length) output << "#binvox 1\n"; output << "dim " << ncells_x << " " << ncells_z << " " << ncells_y << "\n"; - output << "translate " << int(-x_size/2) << " " << int(-y_size / 2) << " " << int(-z_size/2) << "\n"; - output << "scale " << 1/x_size << "\n"; + output << "translate " << -x_size * 0.5 << " " << -y_size * 0.5 << " " << -z_size * 0.5 << "\n"; + output << "scale " << 1.0f/x_size << "\n"; output << "data\n"; bool run_value = voxel_grid_[0]; unsigned int run_length = 0; From c200892b38c637aca37f279fe05904f2c18650e7 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Tue, 15 Dec 2020 14:54:58 -0800 Subject: [PATCH 8/9] Mark unimplemented method in Unity --- Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp | 5 +++++ Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h | 1 + 2 files changed, 6 insertions(+) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index c8e5d7d025..e645e205ef 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -103,6 +103,11 @@ void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) //TODO: implement weather for Unity } +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) +{ + return false; +} + //----------------Plotting APIs-----------/ void WorldSimApi::simFlushPersistentMarkers() { diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index 96a135dd51..2f479655f3 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -62,6 +62,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool isRecording() const override; virtual void setWind(const Vector3r& wind) const override; + virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; private: SimModeBase * simmode_; From c823d6d81407fe129f8a799f1eb05c3eb7276ac6 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Wed, 16 Dec 2020 11:26:23 -0800 Subject: [PATCH 9/9] Fix method name mismatch --- PythonClient/airsim/client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 728bc20274..4509b8fc46 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -790,7 +790,7 @@ def simSetWind(self, wind): """ self.client.call('simSetWind', wind) - def createVoxelGrid(self, position, x, y, z, res, of): + def simCreateVoxelGrid(self, position, x, y, z, res, of): """ Construct and save a binvox-formatted voxel grid of environment