Added an api function: simGetLidarSegmentation().

Returns the segmentation of each lidar point's collided object in the last lidar update. i.e. the segmentation IDs align with getLidarData().point_cloud
microsoft · Sep 13, 2019 · d95a1f8 · d95a1f8
1 parent 09f6310
commit d95a1f8
Show file tree

Hide file tree

Showing 11 changed files with 94 additions and 23 deletions.
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
@@ -75,6 +75,9 @@ class RpcLibClientBase {
     msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;
 
+    // sensor omniscient APIs
+    vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
+
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
 

diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp
@@ -110,26 +110,22 @@ class VehicleApiBase : public UpdatableObject {
     // Lidar APIs
     virtual LidarData getLidarData(const std::string& lidar_name) const
     {
-        const LidarBase* lidar = nullptr;
-
-        // Find lidar with the given name (for empty input name, return the first one found)
-        // Not efficient but should suffice given small number of lidars
-        uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
-        for (uint i = 0; i < count_lidars; i++)
-        {
-            const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
-            if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
-            {
-                lidar = current_lidar;
-                break;
-            }
-        }
+        auto *lidar = findLidarByName(lidar_name);
         if (lidar == nullptr)
             throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
 
         return lidar->getOutput();
     }
 
+    virtual vector<int> getLidarSegmentation(const std::string& lidar_name) const
+    {
+        auto *lidar = findLidarByName(lidar_name);
+        if (lidar == nullptr)
+            throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
+
+        return lidar->getSegmentationOutput();
+    }
+
     // IMU API
     virtual ImuBase::Output getImuData(const std::string& imu_name) const
     {
@@ -260,6 +256,27 @@ class VehicleApiBase : public UpdatableObject {
             : VehicleControllerException(message) {
         }
     };
+
+    private:
+    const LidarBase* findLidarByName(const std::string& lidar_name) const
+    {
+        const LidarBase* lidar = nullptr;
+
+        // Find lidar with the given name (for empty input name, return the first one found)
+        // Not efficient but should suffice given small number of lidars
+        uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
+        for (uint i = 0; i < count_lidars; i++)
+        {
+            const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
+            if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
+            {
+                lidar = current_lidar;
+                break;
+            }
+        }
+
+        return lidar;
+    }
 };
 
 

diff --git a/AirLib/include/sensors/lidar/LidarBase.hpp b/AirLib/include/sensors/lidar/LidarBase.hpp
@@ -45,14 +45,25 @@ class LidarBase : public SensorBase {
         return output_;
     }
 
+    const vector<int>& getSegmentationOutput() const
+    {
+        return segmentation_output_;
+    }
+
 protected:
     void setOutput(const LidarData& output)
     {
         output_ = output;
     }
 
+    void setSegmentationOutput(vector<int>& segmentation_output)
+    {
+        segmentation_output_ = segmentation_output;
+    }
+
 private:
     LidarData output_;
+    vector<int> segmentation_output_;
 };
 
 }} //namespace

diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp
@@ -68,7 +68,7 @@ class LidarSimple : public LidarBase {
 
 protected:
     virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose, 
-        TTimeDelta delta_time, vector<real_T>& point_cloud) = 0;
+        TTimeDelta delta_time, vector<real_T>& point_cloud, vector<int>& segmentation_cloud) = 0;
 
 
 private: //methods
@@ -91,8 +91,10 @@ class LidarSimple : public LidarBase {
         Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose;
         getPointCloud(params_.relative_pose, // relative lidar pose
             ground_truth.kinematics->pose,   // relative vehicle pose
-            delta_time, 
-            point_cloud_);
+            delta_time,
+            point_cloud_,
+            segmentation_cloud_
+        );
 
         LidarData output;
         output.point_cloud = point_cloud_;
@@ -102,11 +104,13 @@ class LidarSimple : public LidarBase {
         last_time_ = output.time_stamp;
 
         setOutput(output);
+        setSegmentationOutput(segmentation_cloud_);
     }
 
 private:
     LidarSimpleParams params_;
     vector<real_T> point_cloud_;
+    vector<int> segmentation_cloud_;
 
     FrequencyLimiter freq_limiter_;
     TTimePoint last_time_;

diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp
@@ -103,7 +103,7 @@ int RpcLibClientBase::getMinRequiredClientVersion() const
 }
 int RpcLibClientBase::getServerVersion() const
 {
-	return pimpl_->client.call("getServerVersion").as<int>();
+    return pimpl_->client.call("getServerVersion").as<int>();
 }
 
 void RpcLibClientBase::reset()
@@ -189,6 +189,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const
     return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as<RpcLibAdapatorsBase::DistanceSensorData>().to();
 }
 
+vector<int> RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const
+{
+    return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as<vector<int>>();
+}
+
 bool RpcLibClientBase::simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex)
 {
     return pimpl_->client.call("simSetSegmentationObjectID", mesh_name, object_id, is_name_regex).as<bool>();

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -126,6 +126,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::Pose(pose);
     });
 
+    pimpl_->server.
+        bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector<int> {
+        return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name);
+    });
+
     pimpl_->server.
         bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool {
         return getWorldSimApi()->setSegmentationObjectID(mesh_name, object_id, is_name_regex);

diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py
@@ -161,6 +161,9 @@ def getDistanceSensorData(self, lidar_name = '', vehicle_name = ''):
 
     def getLidarData(self, lidar_name = '', vehicle_name = ''):
         return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name))
+
+    def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''):
+        return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name)
 
     #----------- APIs to control ACharacter in scene ----------/
     def simCharSetFaceExpression(self, expression_name, value, character_name = ""):

diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp
@@ -42,9 +42,10 @@ void UnrealLidarSensor::createLasers()
 
 // returns a point-cloud for the tick
 void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
-    const msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud)
+    const msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud, vector<int>& segmentation_cloud)
 {
     point_cloud.clear();
+    segmentation_cloud.clear();
 
     msr::airlib::LidarSimpleParams params = getParams();
     const auto number_of_lasers = params.number_of_channels;
@@ -89,12 +90,14 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
                 continue;
 
             Vector3r point;
+            int segmentationID = -1;
             // shoot laser and get the impact point, if any
-            if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point))
+            if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point, segmentationID))
             {
                 point_cloud.emplace_back(point.x());
                 point_cloud.emplace_back(point.y());
                 point_cloud.emplace_back(point.z());
+                segmentation_cloud.emplace_back(segmentationID);
             }
         }
     }
@@ -107,7 +110,7 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
 // simulate shooting a laser via Unreal ray-tracing.
 bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
     const uint32 laser, const float horizontal_angle, const float vertical_angle, 
-    const msr::airlib::LidarSimpleParams params, Vector3r &point)
+    const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID)
 {
     // start position
     Vector3r start = lidar_pose.position + vehicle_pose.position;
@@ -135,6 +138,18 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms
 
     if (is_hit)
     {
+        //Store the segmentation id of the hit object.
+        auto hitActor = hit_result.GetActor();
+        if (hitActor != nullptr)
+        {
+            TArray<UMeshComponent*> meshComponents;
+            hitActor->GetComponents<UMeshComponent>(meshComponents);
+            for (int i = 0; i < meshComponents.Num(); i++)
+            {
+                segmentationID = segmentationID == -1 ? meshComponents[i]->CustomDepthStencilValue : segmentationID;
+            }
+        }
+
         if (false && UAirBlueprintLib::IsInGameThread())
         {
             // Debug code for very specific cases.

diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h
@@ -21,7 +21,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple {
 
 protected:
     virtual void getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
-        msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud) override;
+        msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud, vector<int>& segmentation_cloud) override;
 
 private:
     using Vector3r = msr::airlib::Vector3r;
@@ -30,7 +30,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple {
     void createLasers();
     bool shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
         const uint32 channel, const float horizontal_angle, const float vertical_angle, 
-        const msr::airlib::LidarSimpleParams params, Vector3r &point);
+        const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID);
 
 private:
     AActor* actor_;

diff --git a/UnrealPluginFiles.vcxproj b/UnrealPluginFiles.vcxproj
@@ -95,6 +95,7 @@
     <ClCompile Include="unreal\plugins\airsim\source\simmode\SimModeWorldBase.cpp" />
     <ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealImageCapture.cpp" />
     <ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealDistanceSensor.cpp" />
+    <ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.cpp" />
     <ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealSensorFactory.cpp" />
     <ClCompile Include="unreal\plugins\airsim\source\VehicleCameraConnector.cpp" />
     <ClCompile Include="Unreal\Plugins\AirSim\Source\VehicleSimApi.cpp" />
@@ -128,6 +129,7 @@
     <ClInclude Include="unreal\plugins\airsim\source\simmode\SimModeWorldBase.h" />
     <ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealImageCapture.h" />
     <ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealDistanceSensor.h" />
+    <ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.h" />
     <ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealSensorFactory.h" />
     <ClInclude Include="unreal\plugins\airsim\source\VehicleCameraConnector.h" />
     <ClInclude Include="unreal\plugins\airsim\source\VehiclePawnWrapper.h" />

diff --git a/UnrealPluginFiles.vcxproj.filters b/UnrealPluginFiles.vcxproj.filters
@@ -105,6 +105,9 @@
     <ClCompile Include="Unreal\Plugins\AirSim\Source\Weather\WeatherLib.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="unreal\plugins\airsim\source\AirBlueprintLib.h">
@@ -203,6 +206,9 @@
     <ClInclude Include="Unreal\Plugins\AirSim\Source\Weather\WeatherLib.h">
       <Filter>Source Files</Filter>
     </ClInclude>
+    <ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.h">
+      <Filter>Source Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <None Include="Unreal\Plugins\AirSim\Source\AirSim.Build.4.15.cs">