Merge pull request #2810 from rajat2004/lidar

Combine Lidar Segmentation API into getLidarData

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -465,8 +465,9 @@ class RpcLibAdapatorsBase {
         msr::airlib::TTimePoint time_stamp;    // timestamp
         std::vector<float> point_cloud;        // data
         Pose pose;
+        std::vector<int> segmentation;
 
-        MSGPACK_DEFINE_MAP(time_stamp, point_cloud, pose);
+        MSGPACK_DEFINE_MAP(time_stamp, point_cloud, pose, segmentation);
 
         LidarData()
         {}
@@ -476,6 +477,7 @@ class RpcLibAdapatorsBase {
             time_stamp = s.time_stamp;
             point_cloud = s.point_cloud;
             pose = s.pose;
+            segmentation = s.segmentation;
         }
 
         msr::airlib::LidarData to() const
@@ -485,6 +487,7 @@ class RpcLibAdapatorsBase {
             d.time_stamp = time_stamp;
             d.point_cloud = point_cloud;
             d.pose = pose.to();
+            d.segmentation = segmentation;
 
             return d;
         }

AirLib/include/api/RpcLibClientBase.hpp

@@ -89,9 +89,6 @@ class RpcLibClientBase {
     msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::DistanceSensorData 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 = "");
     void simSetTraceLine(const std::vector<float>& color_rgba, float thickness=3.0f, const std::string& vehicle_name = "");

AirLib/include/api/VehicleApiBase.hpp

@@ -112,39 +112,17 @@ class VehicleApiBase : public UpdatableObject {
     // Lidar APIs
     virtual LidarData getLidarData(const std::string& lidar_name) const
     {
-        auto *lidar = findLidarByName(lidar_name);
+        auto *lidar = static_cast<const LidarBase*>(findSensorByName(lidar_name, SensorBase::SensorType::Lidar));
         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
     {
-        const ImuBase* imu = nullptr;
-
-        // Find imu with the given name (for empty input name, return the first one found)
-        // Not efficient but should suffice given small number of imus
-        uint count_imus = getSensors().size(SensorBase::SensorType::Imu);
-        for (uint i = 0; i < count_imus; i++)
-        {
-            const ImuBase* current_imu = static_cast<const ImuBase*>(getSensors().getByType(SensorBase::SensorType::Imu, i));
-            if (current_imu != nullptr && (current_imu->getName() == imu_name || imu_name == ""))
-            {
-                imu = current_imu;
-                break;
-            }
-        }
+        auto *imu = static_cast<const ImuBase*>(findSensorByName(imu_name, SensorBase::SensorType::Imu));
         if (imu == nullptr)
             throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str()));
 
@@ -154,18 +132,7 @@ class VehicleApiBase : public UpdatableObject {
     // Barometer API
     virtual BarometerBase::Output getBarometerData(const std::string& barometer_name) const
     {
-        const BarometerBase* barometer = nullptr;
-
-        uint count_barometers = getSensors().size(SensorBase::SensorType::Barometer);
-        for (uint i = 0; i < count_barometers; i++)
-        {
-            const BarometerBase* current_barometer = static_cast<const BarometerBase*>(getSensors().getByType(SensorBase::SensorType::Barometer, i));
-            if (current_barometer != nullptr && (current_barometer->getName() == barometer_name || barometer_name == ""))
-            {
-                barometer = current_barometer;
-                break;
-            }
-        }
+        auto *barometer = static_cast<const BarometerBase*>(findSensorByName(barometer_name, SensorBase::SensorType::Barometer));
         if (barometer == nullptr)
             throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str()));
 
@@ -175,18 +142,7 @@ class VehicleApiBase : public UpdatableObject {
     // Magnetometer API
     virtual MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name) const
     {
-        const MagnetometerBase* magnetometer = nullptr;
-
-        uint count_magnetometers = getSensors().size(SensorBase::SensorType::Magnetometer);
-        for (uint i = 0; i < count_magnetometers; i++)
-        {
-            const MagnetometerBase* current_magnetometer = static_cast<const MagnetometerBase*>(getSensors().getByType(SensorBase::SensorType::Magnetometer, i));
-            if (current_magnetometer != nullptr && (current_magnetometer->getName() == magnetometer_name || magnetometer_name == ""))
-            {
-                magnetometer = current_magnetometer;
-                break;
-            }
-        }
+        auto *magnetometer = static_cast<const MagnetometerBase*>(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer));
         if (magnetometer == nullptr)
             throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str()));
 
@@ -196,18 +152,7 @@ class VehicleApiBase : public UpdatableObject {
     // Gps API
     virtual GpsBase::Output getGpsData(const std::string& gps_name) const
     {
-        const GpsBase* gps = nullptr;
-
-        uint count_gps = getSensors().size(SensorBase::SensorType::Gps);
-        for (uint i = 0; i < count_gps; i++)
-        {
-            const GpsBase* current_gps = static_cast<const GpsBase*>(getSensors().getByType(SensorBase::SensorType::Gps, i));
-            if (current_gps != nullptr && (current_gps->getName() == gps_name || gps_name == ""))
-            {
-                gps = current_gps;
-                break;
-            }
-        }
+        auto *gps = static_cast<const GpsBase*>(findSensorByName(gps_name, SensorBase::SensorType::Gps));
         if (gps == nullptr)
             throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str()));
 
@@ -217,18 +162,7 @@ class VehicleApiBase : public UpdatableObject {
     // Distance Sensor API
     virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const
     {
-        const DistanceBase* distance_sensor = nullptr;
-
-        uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
-        for (uint i = 0; i < count_distance_sensors; i++)
-        {
-            const DistanceBase* current_distance_sensor = static_cast<const DistanceBase*>(getSensors().getByType(SensorBase::SensorType::Distance, i));
-            if (current_distance_sensor != nullptr && (current_distance_sensor->getName() == distance_sensor_name || distance_sensor_name == ""))
-            {
-                distance_sensor = current_distance_sensor;
-                break;
-            }
-        }
+        auto *distance_sensor = static_cast<const DistanceBase*>(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance));
         if (distance_sensor == nullptr)
             throw VehicleControllerException(Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str()));
 
@@ -260,24 +194,24 @@ class VehicleApiBase : public UpdatableObject {
     };
 
     private:
-    const LidarBase* findLidarByName(const std::string& lidar_name) const
+    const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) const
     {
-        const LidarBase* lidar = nullptr;
+        const SensorBase* sensor = 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++)
+        // Find sensor with the given name (for empty input name, return the first one found)
+        // Not efficient but should suffice given small number of sensors
+        uint count_sensors = getSensors().size(type);
+        for (uint i = 0; i < count_sensors; 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 == ""))
+            const SensorBase* current_sensor = getSensors().getByType(type, i);
+            if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == ""))
             {
-                lidar = current_lidar;
+                sensor = current_sensor;
                 break;
             }
         }
 
-        return lidar;
+        return sensor;
     }
 };
 

AirLib/include/common/CommonStructs.hpp

@@ -297,8 +297,15 @@ struct RCData {
 
 struct LidarData {
     TTimePoint time_stamp = 0;
+    // data
+    // - array of floats that represent [x,y,z] coordinate for each point hit within the range
+    //       x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
+    //       TODO: Do we need an intensity place-holder [x,y,z, intensity]?
+    // - in lidar local NED coordinates
+    // - in meters
     vector<real_T> point_cloud;
     Pose pose;
+    vector<int> segmentation;
 
     LidarData()
     {}

AirLib/include/sensors/lidar/LidarBase.hpp

@@ -14,22 +14,6 @@ class LidarBase : public SensorBase {
         : SensorBase(sensor_name)
     {}
 
-public: //types
-    struct Output { //fields to enable creation of ROS message PointCloud2 and LaserScan
-
-        // header
-        TTimePoint time_stamp;
-        Pose relative_pose;
-
-        // data
-        // - array of floats that represent [x,y,z] coordinate for each point hit within the range
-        //       x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
-        //       TODO: Do we need an intensity place-holder [x,y,z, intensity]?
-        // - in lidar local NED coordinates
-        // - in meters
-        vector<real_T> point_cloud;
-    };
-
 public:
     virtual void reportState(StateReporter& reporter) override
     {
@@ -45,25 +29,14 @@ 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

AirLib/include/sensors/lidar/LidarSimple.hpp

@@ -97,12 +97,12 @@ class LidarSimple : public LidarBase {
         LidarData output;
         output.point_cloud = point_cloud_;
         output.time_stamp = clock()->nowNanos();
-        output.pose = lidar_pose;            
+        output.pose = lidar_pose;
+        output.segmentation = segmentation_cloud_;
 
         last_time_ = output.time_stamp;
 
         setOutput(output);
-        setSegmentationOutput(segmentation_cloud_);
     }
 
 private:

AirLib/src/api/RpcLibClientBase.cpp

@@ -189,11 +189,6 @@ msr::airlib::DistanceSensorData RpcLibClientBase::getDistanceSensorData(const st
     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>();

AirLib/src/api/RpcLibServerBase.cpp

@@ -167,11 +167,6 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness);
     });
 
-    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);

PythonClient/airsim/client.py

@@ -669,6 +669,7 @@ def getLidarData(self, lidar_name = '', vehicle_name = ''):
 
     def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''):
         """
+        NOTE: Deprecated API, use `getLidarData()` API instead
         Returns Segmentation ID of each point's collided object in the last Lidar update
 
         Args:
@@ -678,7 +679,8 @@ def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''):
         Returns:
             list[int]: Segmentation IDs of the objects
         """
-        return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name)
+        logging.warning("simGetLidarSegmentation API is deprecated, use getLidarData() API instead")
+        return self.getLidarData(lidar_name, vehicle_name).segmentation
 
     #  Plotting APIs
     def simFlushPersistentMarkers(self):

PythonClient/airsim/types.py

@@ -371,6 +371,7 @@ class LidarData(MsgpackMixin):
     point_cloud = 0.0
     time_stamp = np.uint64(0)
     pose = Pose()
+    segmentation = 0
 
 class ImuData(MsgpackMixin):
     time_stamp = np.uint64(0)