Merge pull request #3209 from saihv/PR/voxel_grid

Voxel grid construction

Author: zimmy87

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;
 

AirLib/include/api/WorldSimApiBase.hpp

@@ -67,6 +67,8 @@ class WorldSimApiBase {
 	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 vector<MeshPositionVertexBuffersResponse> 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;

AirLib/src/api/RpcLibClientBase.cpp

@@ -406,6 +406,11 @@ msr::airlib::Environment::State RpcLibClientBase::simGetGroundTruthEnvironment(c
 {
     return pimpl_->client.call("simGetGroundTruthEnvironment", vehicle_name).as<RpcLibAdapatorsBase::EnvironmentState>().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<bool>();
+}
+
 
 void RpcLibClientBase::cancelLastTask(const std::string& vehicle_name)
 {

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();

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)

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()
 {

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_;

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
+		}
 	]
-}
+}

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<char>(run_value);
+                output << static_cast<char>(run_length);
+                run_length = 0;
+            }
+        }
+        else
+        {
+            // End of a run
+            output << static_cast<char>(run_value);
+            output << static_cast<char>(run_length);
+            run_value = voxel_grid_[i];
+            run_length = 1;
+        }
+    }
+    if (run_length > 0)
+    {
+        output << static_cast<char>(run_value);
+        output << static_cast<char>(run_length);
+    }
+    output.close();
+    success = true;
+    return success;
+}
+
 bool WorldSimApi::isPaused() const
 {
     return simmode_->isPaused();

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<bool> voxel_grid_;
+};