diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index b14e18f5d1..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 = ""); - + 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 286b98d99d..cf27afbe1a 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 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; virtual void stopRecording() = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 04b6e33ce1..18b1c18015 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();; } +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).as(); +} + void RpcLibClientBase::cancelLastTask(const std::string& vehicle_name) { diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 52baf8666f..80e2c47af5 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) -> bool { + 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..4509b8fc46 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 simCreateVoxelGrid(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 + + """ + return 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/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_; 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..935782dd4c 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; } +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; + + voxel_grid_.resize(ncells_x * ncells_y * ncells_z); + + float scale_cm = res * 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] = simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params); + } + } + } + + std::ofstream output(output_file, std::ios::out | std::ios::binary); + if (!output.good()) + { + UE_LOG(LogTemp, Error, TEXT("Could not open output file to write voxel grid!")); + return success; + } + + // 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 " << -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; + 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(); + success = true; + return success; +} + 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..2a96852343 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 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); @@ -73,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