Merge pull request #1479 from bmalhigh/feature/lidar-config

Add support for configuration settings for Sensors.

AirLib/include/api/VehicleApiBase.hpp

@@ -107,13 +107,13 @@ class VehicleApiBase : public UpdatableObject {
     {
         const LidarBase* lidar = nullptr;
 
-        // Find lidar with the given name
+        // 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)
+            if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
             {
                 lidar = current_lidar;
                 break;

AirLib/include/common/AirSimSettings.hpp

@@ -12,6 +12,7 @@
 #include "CommonStructs.hpp"
 #include "common_utils/Utils.hpp"
 #include "ImageCaptureBase.hpp"
+#include "sensors/SensorBase.hpp"
 
 namespace msr { namespace airlib {
 
@@ -177,6 +178,44 @@ struct AirSimSettings {
         float follow_distance = Utils::nan<float>();
     };
 
+    struct SensorSetting {
+        SensorBase::SensorType sensor_type;
+        std::string sensor_name;
+        bool enabled;
+    };
+
+    struct BarometerSetting : SensorSetting {
+    };
+
+    struct ImuSetting : SensorSetting {
+    };
+
+    struct GpsSetting : SensorSetting {
+    };
+
+    struct MagnetometerSetting : SensorSetting {
+    };
+
+    struct DistanceSetting : SensorSetting {
+    };
+
+    struct LidarSetting : SensorSetting {
+
+        // shared defaults
+        uint number_of_channels = 16;
+        real_T range = 10000.0f / 100;                    // meters
+        uint points_per_second = 100000;
+        uint horizontal_rotation_frequency = 10;          // rotations/sec
+
+        // defaults specific to a mode
+        float vertical_FOV_upper = Utils::nan<float>();   // drones -15, car +10
+        float vertical_FOV_lower = Utils::nan<float>();   // drones -45, car -10
+        Vector3r position = VectorMath::nanVector(); 
+        Rotation rotation = Rotation::nanRotation();
+
+        bool draw_debug_points = false;
+    };
+
     struct VehicleSetting {
         //required
         std::string vehicle_name;
@@ -197,6 +236,7 @@ struct AirSimSettings {
         Rotation rotation = Rotation::nanRotation();
 
         std::map<std::string, CameraSetting> cameras;
+        std::map<std::string, std::unique_ptr<SensorSetting>> sensors;
 
         RCSettings rc;
     };
@@ -299,8 +339,9 @@ struct AirSimSettings {
     std::map<std::string, std::unique_ptr<VehicleSetting>> vehicles;
     CameraSetting camera_defaults;
     CameraDirectorSetting camera_director;
-	  float speed_unit_factor =  1.0f;
-	  std::string speed_unit_label = "m\\s";
+	float speed_unit_factor =  1.0f;
+	std::string speed_unit_label = "m\\s";
+    std::map<std::string, std::unique_ptr<SensorSetting>> sensor_defaults;
 
 public: //methods
     static AirSimSettings& singleton() 
@@ -333,6 +374,7 @@ struct AirSimSettings {
         loadSegmentationSetting(settings_json, segmentation_setting);
         loadPawnPaths(settings_json, pawn_paths);
         loadOtherSettings(settings_json);
+        loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults);
         loadVehicleSettings(simmode_name, settings_json, vehicles);
 
         //this should be done last because it depends on type of vehicles we have
@@ -670,7 +712,8 @@ struct AirSimSettings {
         vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation);
 
         loadCameraSettings(settings_json, vehicle_setting->cameras);
-
+        loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors);
+
         return vehicle_setting;
     }
 
@@ -1040,6 +1083,177 @@ struct AirSimSettings {
 
         clock_speed = settings_json.getFloat("ClockSpeed", 1.0f);
     }
+
+    static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json)
+    {
+        unused(barometer_setting);
+        unused(settings_json);
+
+        //TODO: set from json as needed
+    }
+
+    static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json)
+    {
+        unused(imu_setting);
+        unused(settings_json);
+
+        //TODO: set from json as needed
+    }
+
+    static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json)
+    {
+        unused(gps_setting);
+        unused(settings_json);
+
+        //TODO: set from json as needed
+    }
+
+    static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json)
+    {
+        unused(magnetometer_setting);
+        unused(settings_json);
+
+        //TODO: set from json as needed
+    }
+
+    static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json)
+    {
+        unused(distance_setting);
+        unused(settings_json);
+
+        //TODO: set from json as needed
+    }
+
+    static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json)
+    {
+        lidar_setting.number_of_channels = settings_json.getInt("NumberOfChannels", lidar_setting.number_of_channels);
+        lidar_setting.range = settings_json.getFloat("Range", lidar_setting.range);
+        lidar_setting.points_per_second = settings_json.getInt("PointsPerSecond", lidar_setting.points_per_second);
+        lidar_setting.horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", lidar_setting.horizontal_rotation_frequency);
+        lidar_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", lidar_setting.draw_debug_points);
+
+        lidar_setting.vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", lidar_setting.vertical_FOV_upper);
+        lidar_setting.vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", lidar_setting.vertical_FOV_lower);
+
+        lidar_setting.position = createVectorSetting(settings_json, lidar_setting.position);
+        lidar_setting.rotation = createRotationSetting(settings_json, lidar_setting.rotation);
+    }
+
+    static std::unique_ptr<SensorSetting> createSensorSetting(
+        SensorBase::SensorType sensor_type, const std::string& sensor_name,
+        bool enabled)
+    {
+        std::unique_ptr<SensorSetting> sensor_setting;
+
+        switch (sensor_type) {
+        case SensorBase::SensorType::Barometer:
+            sensor_setting = std::unique_ptr<SensorSetting>(new BarometerSetting());
+            break;
+        case SensorBase::SensorType::Imu:
+            sensor_setting = std::unique_ptr<SensorSetting>(new ImuSetting());
+            break;
+        case SensorBase::SensorType::Gps:
+            sensor_setting = std::unique_ptr<SensorSetting>(new GpsSetting());
+            break;
+        case SensorBase::SensorType::Magnetometer:
+            sensor_setting = std::unique_ptr<SensorSetting>(new MagnetometerSetting());
+            break;
+        case SensorBase::SensorType::Distance:
+            sensor_setting = std::unique_ptr<SensorSetting>(new DistanceSetting());
+            break;
+        case SensorBase::SensorType::Lidar:
+            sensor_setting = std::unique_ptr<SensorSetting>(new LidarSetting());
+            break;
+        default:
+            throw std::invalid_argument("Unexpected sensor type");
+        }
+
+        sensor_setting->sensor_type = sensor_type;
+        sensor_setting->sensor_name = sensor_name;
+        sensor_setting->enabled = enabled;
+
+        return sensor_setting;
+    }
+
+    static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json)
+    {
+        sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled);
+
+        switch (sensor_setting->sensor_type) {
+        case SensorBase::SensorType::Barometer:
+            initializeBarometerSetting(*static_cast<BarometerSetting*>(sensor_setting), settings_json);
+            break;
+        case SensorBase::SensorType::Imu:
+            initializeImuSetting(*static_cast<ImuSetting*>(sensor_setting), settings_json);
+            break;
+        case SensorBase::SensorType::Gps:
+            initializeGpsSetting(*static_cast<GpsSetting*>(sensor_setting), settings_json);
+            break;
+        case SensorBase::SensorType::Magnetometer:
+            initializeMagnetometerSetting(*static_cast<MagnetometerSetting*>(sensor_setting), settings_json);
+            break;
+        case SensorBase::SensorType::Distance:
+            initializeDistanceSetting(*static_cast<DistanceSetting*>(sensor_setting), settings_json);
+            break;
+        case SensorBase::SensorType::Lidar:
+            initializeLidarSetting(*static_cast<LidarSetting*>(sensor_setting), settings_json);
+            break;
+        default:
+            throw std::invalid_argument("Unexpected sensor type");
+        }
+    }
+
+    // creates and intializes sensor settings from json
+    static void loadSensorSettings( const Settings& settings_json, const std::string& collectionName,
+        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+    {
+        msr::airlib::Settings sensors_child;
+        if (settings_json.getChild(collectionName, sensors_child)) {
+            std::vector<std::string> keys;
+            sensors_child.getChildNames(keys);
+
+            for (const auto& key : keys) {
+                msr::airlib::Settings child;
+                sensors_child.getChild(key, child);
+
+                auto sensor_type = Utils::toEnum<SensorBase::SensorType>(child.getInt("SensorType", 0));
+                auto enabled = child.getBool("Enabled", false);
+
+                sensors[key] = createSensorSetting(sensor_type, key, enabled);
+                initializeSensorSetting(sensors[key].get(), child);
+            }
+        }
+    }
+
+    // creates default sensor list when none specified in json
+    static void createDefaultSensorSettings(const std::string& simmode_name,
+        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+    {
+        if (simmode_name == "Multirotor") {
+            sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true);
+            sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true);
+            sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
+            sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true);
+        }
+        else if (simmode_name == "Car") {
+            sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
+        }
+        else {
+            // no sensors added for other modes
+        }
+    }
+
+    // loads or creates default sensor list
+    static void loadDefaultSensorSettings(const std::string& simmode_name, 
+        const Settings& settings_json,
+        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+    {
+        msr::airlib::Settings sensors_child;
+        if (settings_json.getChild("DefaultSensors", sensors_child))
+            loadSensorSettings(settings_json, "DefaultSensors", sensors);
+        else
+            createDefaultSensorSettings(simmode_name, sensors);
+    }
 };
 
 }} //namespace

AirLib/include/sensors/SensorBase.hpp

@@ -28,6 +28,11 @@ class SensorBase : public UpdatableObject  {
         Distance = 5,
         Lidar = 6
     };
+
+    SensorBase(const std::string& sensor_name = "")
+        : name_(sensor_name)
+    {}
+
 protected:
     struct GroundTruth {
         const Kinematics::State* kinematics;
@@ -44,12 +49,18 @@ class SensorBase : public UpdatableObject  {
     {
         return ground_truth_;
     }
-
+
+    const std::string& getName() const
+    {
+        return name_;
+    }
+
     virtual ~SensorBase() = default;
 
 private:
     //ground truth can be shared between many sensors
     GroundTruth ground_truth_;
+    std::string name_ = "";
 };
 
 

AirLib/include/sensors/SensorFactory.hpp

@@ -3,6 +3,7 @@
 
 
 #include "SensorBase.hpp"
+#include "SensorCollection.hpp"
 #include <memory>
 
 //sensors
@@ -16,19 +17,44 @@ namespace msr { namespace airlib {
 
 class SensorFactory {
 public:
-    virtual std::unique_ptr<SensorBase> createSensor(SensorBase::SensorType sensor_type) const
+
+    // creates one sensor from settings
+    virtual std::unique_ptr<SensorBase> createSensorFromSettings(
+        const AirSimSettings::SensorSetting* sensor_setting) const
     {
-        switch (sensor_type) {
+        switch (sensor_setting->sensor_type) {
         case SensorBase::SensorType::Imu:
-            return std::unique_ptr<ImuSimple>(new ImuSimple());
+            return std::unique_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
         case SensorBase::SensorType::Magnetometer:
-            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple());
+            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
         case SensorBase::SensorType::Gps:
-            return std::unique_ptr<GpsSimple>(new GpsSimple());
+            return std::unique_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
         case SensorBase::SensorType::Barometer:
-            return std::unique_ptr<BarometerSimple>(new BarometerSimple());
+            return std::unique_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
         default:
-            return std::unique_ptr<SensorBase>();
+            throw new std::invalid_argument("Unexpected sensor type");
+        }
+    }
+
+    // creates sensor-collection
+    virtual void createSensorsFromSettings(
+        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
+        SensorCollection& sensors,
+        vector<unique_ptr<SensorBase>>& sensor_storage) const
+    {
+        for (const auto& sensor_setting_pair : sensors_settings) {
+            const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
+
+            // ignore sensors that are marked "disabled" in settings
+            if (sensor_setting == nullptr || !sensor_setting->enabled)
+                continue;
+
+            std::unique_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
+            if (sensor) {
+                    SensorBase* sensor_temp = sensor.get();
+                    sensor_storage.push_back(std::move(sensor));
+                    sensors.insert(sensor_temp, sensor_setting->sensor_type);
+            }
         }
     }
 };

AirLib/include/sensors/barometer/BarometerBase.hpp

@@ -11,6 +11,11 @@
 namespace msr { namespace airlib {
 
 class BarometerBase  : public SensorBase {
+public:
+    BarometerBase(const std::string& sensor_name = "")
+        : SensorBase(sensor_name)
+    {}
+
 public: //types
     struct Output { //same fields as ROS message
         real_T altitude;    //meters