From b1ac0f1640f8a9312ff26cdfc544f30737e480c6 Mon Sep 17 00:00:00 2001 From: Balinder Malhi Date: Thu, 18 Oct 2018 13:47:10 -0700 Subject: [PATCH 1/2] Add support for configuration settings for Sensors. Validated on Windows and Linux. --- AirLib/include/api/VehicleApiBase.hpp | 4 +- AirLib/include/common/AirSimSettings.hpp | 172 +++++++++++++++++- AirLib/include/sensors/SensorBase.hpp | 13 +- AirLib/include/sensors/SensorFactory.hpp | 43 +++++ .../sensors/barometer/BarometerBase.hpp | 5 + .../sensors/barometer/BarometerSimple.hpp | 8 +- .../barometer/BarometerSimpleParams.hpp | 4 + .../include/sensors/distance/DistanceBase.hpp | 5 + .../sensors/distance/DistanceSimple.hpp | 8 +- .../sensors/distance/DistanceSimpleParams.hpp | 5 + AirLib/include/sensors/gps/GpsBase.hpp | 5 + AirLib/include/sensors/gps/GpsSimple.hpp | 8 +- .../include/sensors/gps/GpsSimpleParams.hpp | 5 + AirLib/include/sensors/imu/ImuBase.hpp | 5 + AirLib/include/sensors/imu/ImuSimple.hpp | 8 +- .../include/sensors/imu/ImuSimpleParams.hpp | 6 + AirLib/include/sensors/lidar/LidarBase.hpp | 11 +- AirLib/include/sensors/lidar/LidarSimple.hpp | 23 ++- .../sensors/lidar/LidarSimpleParams.hpp | 55 +++++- .../sensors/magnetometer/MagnetometerBase.hpp | 5 + .../magnetometer/MagnetometerSimple.hpp | 8 +- .../magnetometer/MagnetometerSimpleParams.hpp | 5 + .../include/vehicles/car/api/CarApiBase.hpp | 67 ++----- .../vehicles/multirotor/MultiRotorParams.hpp | 47 +---- .../multirotor/MultiRotorParamsFactory.hpp | 2 +- .../mavlink/ArduCopterSoloParams.hpp | 8 +- .../firmwares/mavlink/Px4MultiRotorParams.hpp | 4 +- .../simple_flight/SimpleFlightQuadXParams.hpp | 5 +- .../AirSim/Source/SimMode/SimModeBase.cpp | 84 +++++---- .../AirSim/Source/SimMode/SimModeBase.h | 3 +- .../UnrealSensors/UnrealDistanceSensor.cpp | 5 +- .../UnrealSensors/UnrealDistanceSensor.h | 6 +- .../UnrealSensors/UnrealLidarSensor.cpp | 9 +- .../Source/UnrealSensors/UnrealLidarSensor.h | 6 +- .../UnrealSensors/UnrealSensorFactory.cpp | 19 +- .../UnrealSensors/UnrealSensorFactory.h | 5 + .../AirSim/Source/Vehicles/Car/CarPawnApi.cpp | 5 +- .../AirSim/Source/Vehicles/Car/CarPawnApi.h | 3 +- .../Source/Vehicles/Car/CarPawnSimApi.cpp | 3 +- 39 files changed, 507 insertions(+), 185 deletions(-) diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index f1ec69daf4..8524a3b59e 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/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(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; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 9005513534..4e5bb98b1a 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/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(); }; + 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(); // drones -15, car +10 + float vertical_FOV_lower = Utils::nan(); // 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 cameras; + std::map> sensors; RCSettings rc; }; @@ -299,8 +339,9 @@ struct AirSimSettings { std::map> 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> 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,129 @@ struct AirSimSettings { clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); } + + 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 createSensorSetting( + SensorBase::SensorType sensor_type, const std::string sensor_name, + bool enabled) + { + std::unique_ptr sensor_setting; + + switch (sensor_type) { + case SensorBase::SensorType::Barometer: + sensor_setting = std::unique_ptr(new BarometerSetting()); + break; + case SensorBase::SensorType::Imu: + sensor_setting = std::unique_ptr(new ImuSetting()); + break; + case SensorBase::SensorType::Gps: + sensor_setting = std::unique_ptr(new GpsSetting()); + break; + case SensorBase::SensorType::Magnetometer: + sensor_setting = std::unique_ptr(new MagnetometerSetting()); + break; + case SensorBase::SensorType::Distance: + sensor_setting = std::unique_ptr(new DistanceSetting()); + break; + case SensorBase::SensorType::Lidar: + sensor_setting = std::unique_ptr(new LidarSetting()); + break; + default: + break; + } + + 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: + break; + case SensorBase::SensorType::Imu: + break; + case SensorBase::SensorType::Gps: + break; + case SensorBase::SensorType::Magnetometer: + break; + case SensorBase::SensorType::Distance: + break; + case SensorBase::SensorType::Lidar: + initializeLidarSetting(static_cast(sensor_setting), settings_json); + break; + default: + break; + } + } + + // creates and intializes sensor settings from json + static void loadSensorSettings( const Settings& settings_json, const std::string collectionName, + std::map>& sensors) + { + msr::airlib::Settings sensors_child; + if (settings_json.getChild(collectionName, sensors_child)) { + std::vector keys; + sensors_child.getChildNames(keys); + + for (const auto& key : keys) { + msr::airlib::Settings child; + sensors_child.getChild(key, child); + + auto sensor_type = Utils::toEnum(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>& 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); + } + } + + // loads or creates default sensor list + static void loadDefaultSensorSettings(const std::string& simmode_name, + const Settings& settings_json, + std::map>& sensors) + { + msr::airlib::Settings sensors_child; + if (settings_json.getChild("DefaultSensors", sensors_child)) + loadSensorSettings(settings_json, "DefaultSensors", sensors); + else + createDefaultSensorSettings(simmode_name, sensors); + } }; }} //namespace diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index c3cd1d04b0..765cfabcf1 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/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_ = ""; }; diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp index 5eef521c31..ad9cec54c2 100644 --- a/AirLib/include/sensors/SensorFactory.hpp +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -3,6 +3,7 @@ #include "SensorBase.hpp" +#include "SensorCollection.hpp" #include //sensors @@ -16,6 +17,8 @@ namespace msr { namespace airlib { class SensorFactory { public: + // creates one sensor + // TODO: Can we remove this function alltogether and require the one that accepts settings only? virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) const { switch (sensor_type) { @@ -31,6 +34,46 @@ class SensorFactory { return std::unique_ptr(); } } + + // creates one sensor from settings + virtual std::unique_ptr createSensorFromSettings( + AirSimSettings::SensorSetting* sensor_setting) const + { + switch (sensor_setting->sensor_type) { + case SensorBase::SensorType::Imu: + return std::unique_ptr(new ImuSimple(sensor_setting)); + case SensorBase::SensorType::Magnetometer: + return std::unique_ptr(new MagnetometerSimple(sensor_setting)); + case SensorBase::SensorType::Gps: + return std::unique_ptr(new GpsSimple(sensor_setting)); + case SensorBase::SensorType::Barometer: + return std::unique_ptr(new BarometerSimple(sensor_setting)); + default: + return std::unique_ptr(); + } + } + + // creates sensor-collection + virtual void createSensorsFromSettings( + const std::map>& sensors_settings, + SensorCollection& sensors, + vector>& sensor_storage) const + { + for (auto& sensor_setting_pair : sensors_settings) { + 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 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); + } + } + } }; diff --git a/AirLib/include/sensors/barometer/BarometerBase.hpp b/AirLib/include/sensors/barometer/BarometerBase.hpp index f69bc77699..eac67ca77c 100644 --- a/AirLib/include/sensors/barometer/BarometerBase.hpp +++ b/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 diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index 31710fbb7f..ea78fcdb36 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -18,9 +18,13 @@ namespace msr { namespace airlib { class BarometerSimple : public BarometerBase { public: - BarometerSimple(const BarometerSimpleParams& params = BarometerSimpleParams()) - : params_(params) + BarometerSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : BarometerBase(sensor_setting != nullptr ? sensor_setting->sensor_name: "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + //GM process that would do random walk for pressure factor pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0); diff --git a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp index 59be590675..62082342dd 100644 --- a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp @@ -47,6 +47,10 @@ struct BarometerSimpleParams { real_T update_frequency = 50; //Hz real_T startup_delay = 0; //sec + void initializeFromSettings(const AirSimSettings::BarometerSetting& settings) + { + unused(settings); + } }; diff --git a/AirLib/include/sensors/distance/DistanceBase.hpp b/AirLib/include/sensors/distance/DistanceBase.hpp index 36147705f1..c71def1073 100644 --- a/AirLib/include/sensors/distance/DistanceBase.hpp +++ b/AirLib/include/sensors/distance/DistanceBase.hpp @@ -11,6 +11,11 @@ namespace msr { namespace airlib { class DistanceBase : public SensorBase { +public: + DistanceBase(const std::string& sensor_name = "") + : SensorBase(sensor_name) + {} + public: //types struct Output { //same fields as ROS message real_T distance; //meters diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 201db6dcb6..cc12f0e9e7 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -16,9 +16,13 @@ namespace msr { namespace airlib { class DistanceSimple : public DistanceBase { public: - DistanceSimple(const DistanceSimpleParams& params = DistanceSimpleParams()) - : params_(params) + DistanceSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : DistanceBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma); //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f); diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index d5f36bec35..8844ab15b0 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -36,6 +36,11 @@ struct DistanceSimpleParams { real_T update_latency = 0.0f; //sec real_T update_frequency = 50; //Hz real_T startup_delay = 0; //sec + + void initializeFromSettings(const AirSimSettings::DistanceSetting& settings) + { + unused(settings); + } }; diff --git a/AirLib/include/sensors/gps/GpsBase.hpp b/AirLib/include/sensors/gps/GpsBase.hpp index b503422319..1ee816023f 100644 --- a/AirLib/include/sensors/gps/GpsBase.hpp +++ b/AirLib/include/sensors/gps/GpsBase.hpp @@ -12,6 +12,11 @@ namespace msr { namespace airlib { class GpsBase : public SensorBase { +public: + GpsBase(const std::string& sensor_name = "") + : SensorBase(sensor_name) + {} + public: //types //TODO: cleanup GPS structures that are not needed struct GpsPoint { diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index d2df628a67..718fcafa5f 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -17,9 +17,13 @@ namespace msr { namespace airlib { class GpsSimple : public GpsBase { public: //methods - GpsSimple(const GpsSimpleParams& params = GpsSimpleParams()) - : params_(params) + GpsSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : GpsBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + //initialize frequency limiter freq_limiter_.initialize(params_.update_frequency, params_.startup_delay); delay_line_.initialize(params_.update_latency); diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp index 59dd42d8ec..52d8cf88c8 100644 --- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp +++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp @@ -18,6 +18,11 @@ struct GpsSimpleParams { real_T update_latency = 0.2f; //sec real_T update_frequency = 50; //Hz real_T startup_delay = 1; //sec + + void initializeFromSettings(const AirSimSettings::GpsSetting& settings) + { + unused(settings); + } }; }} //namespace diff --git a/AirLib/include/sensors/imu/ImuBase.hpp b/AirLib/include/sensors/imu/ImuBase.hpp index e8454218a7..73de2e50f5 100644 --- a/AirLib/include/sensors/imu/ImuBase.hpp +++ b/AirLib/include/sensors/imu/ImuBase.hpp @@ -11,6 +11,11 @@ namespace msr { namespace airlib { class ImuBase : public SensorBase { +public: + ImuBase(const std::string& sensor_name = "") + : SensorBase(sensor_name) + {} + public: //types struct Output { //structure is same as ROS IMU message EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 1aab8cb528..6346ebca20 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -14,9 +14,13 @@ namespace msr { namespace airlib { class ImuSimple : public ImuBase { public: //constructors - ImuSimple(const ImuSimpleParams& params = ImuSimpleParams()) - : params_(params) + ImuSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : ImuBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + gyro_bias_stability_norm = params_.gyro.bias_stability / sqrt(params_.gyro.tau); accel_bias_stability_norm = params_.accel.bias_stability / sqrt(params_.accel.tau); } diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index 759139f17c..6cbe166a4e 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -6,6 +6,7 @@ #include "common/Common.hpp" #include "common/EarthUtils.hpp" +#include "common/AirSimSettings.hpp" #include @@ -42,6 +43,11 @@ struct ImuSimpleParams { } accel; real_T min_sample_time = 1 / 1000.0f; //internal IMU frequency + + void initializeFromSettings(const AirSimSettings::ImuSetting& settings) + { + unused(settings); + } }; diff --git a/AirLib/include/sensors/lidar/LidarBase.hpp b/AirLib/include/sensors/lidar/LidarBase.hpp index 93bbf1d0d8..05a0735e58 100644 --- a/AirLib/include/sensors/lidar/LidarBase.hpp +++ b/AirLib/include/sensors/lidar/LidarBase.hpp @@ -9,6 +9,11 @@ namespace msr { namespace airlib { class LidarBase : public SensorBase { +public: + LidarBase(const std::string& sensor_name = "") + : SensorBase(sensor_name) + {} + public: //types struct Output { //fields to enable creation of ROS message PointCloud2 and LaserScan @@ -40,11 +45,6 @@ class LidarBase : public SensorBase { return output_; } - const std::string& getName() const - { - return name_; - } - protected: void setOutput(const LidarData& output) { @@ -53,7 +53,6 @@ class LidarBase : public SensorBase { private: LidarData output_; - std::string name_ = ""; }; }} //namespace diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp index dabf7d8a5f..42b8f4852b 100644 --- a/AirLib/include/sensors/lidar/LidarSimple.hpp +++ b/AirLib/include/sensors/lidar/LidarSimple.hpp @@ -15,9 +15,13 @@ namespace msr { namespace airlib { class LidarSimple : public LidarBase { public: - LidarSimple(const LidarSimpleParams& params = LidarSimpleParams()) - : params_(params) + LidarSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : LidarBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + //initialize frequency limiter freq_limiter_.initialize(params_.update_frequency, params_.startup_delay); } @@ -51,22 +55,23 @@ class LidarSimple : public LidarBase { reporter.writeValue("Lidar-NumChannels", params_.number_of_channels); reporter.writeValue("Lidar-Range", params_.range); - reporter.writeValue("Lidar-FOV-Upper", params_.vertical_FOV_Upper); - reporter.writeValue("Lidar-FOV-Lower", params_.vertical_FOV_Lower); + reporter.writeValue("Lidar-FOV-Upper", params_.vertical_FOV_upper); + reporter.writeValue("Lidar-FOV-Lower", params_.vertical_FOV_lower); } //*** End: UpdatableState implementation ***// virtual ~LidarSimple() = default; -protected: - virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose, - TTimeDelta delta_time, vector& point_cloud) = 0; - - const LidarSimpleParams& getParams() + const LidarSimpleParams& getParams() const { return params_; } +protected: + virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose, + TTimeDelta delta_time, vector& point_cloud) = 0; + + private: //methods void updateOutput() { diff --git a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp index 7a286c4a8d..6df42277ba 100644 --- a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp +++ b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp @@ -20,16 +20,67 @@ struct LidarSimpleParams { real_T range = 10000.0f / 100; // meters uint points_per_second = 100000; uint horizontal_rotation_frequency = 10; // rotations/sec - int vertical_FOV_Upper = -15; // drones -15, car +10 - int vertical_FOV_Lower = -45; // drones -45, car -10 + real_T vertical_FOV_upper = -15; // drones -15, car +10 + real_T vertical_FOV_lower = -45; // drones -45, car -10 Pose relative_pose { Vector3r(0,0,-1), // position - a little above vehicle (especially for cars) or Vector3r::Zero() Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0) }; + bool draw_debug_points = false; + real_T update_frequency = 10; // Hz real_T startup_delay = 0; // sec + + void initializeFromSettings(const AirSimSettings::LidarSetting& settings) + { + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + number_of_channels = settings.number_of_channels; + range = settings.range; + points_per_second = settings.points_per_second; + horizontal_rotation_frequency = settings.horizontal_rotation_frequency; + + vertical_FOV_upper = settings.vertical_FOV_upper; + if (std::isnan(vertical_FOV_upper)) { + if (simmode_name == "Multirotor") + vertical_FOV_upper = -15; + else + vertical_FOV_upper = +10; + } + + vertical_FOV_lower = settings.vertical_FOV_lower; + if (std::isnan(vertical_FOV_lower)) { + if (simmode_name == "Multirotor") + vertical_FOV_lower = -45; + else + vertical_FOV_lower = -10; + } + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) { + if (simmode_name == "Multirotor") + relative_pose.position.z() = 0; + else + relative_pose.position.z() = -1; // a little bit above for cars + } + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis + + draw_debug_points = settings.draw_debug_points; + } }; }} //namespace diff --git a/AirLib/include/sensors/magnetometer/MagnetometerBase.hpp b/AirLib/include/sensors/magnetometer/MagnetometerBase.hpp index dd14ede056..9457b69416 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerBase.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerBase.hpp @@ -11,6 +11,11 @@ namespace msr { namespace airlib { class MagnetometerBase : public SensorBase { +public: + MagnetometerBase(const std::string& sensor_name = "") + : SensorBase(sensor_name) + {} + public: //types struct Output { //same fields as ROS message Vector3r magnetic_field_body; //in Gauss diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 5cd3f64e26..47e67e6dec 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -17,9 +17,13 @@ namespace msr { namespace airlib { class MagnetometerSimple : public MagnetometerBase { public: - MagnetometerSimple(const MagnetometerSimpleParams& params = MagnetometerSimpleParams()) - : params_(params) + MagnetometerSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) + : MagnetometerBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") { + // initialize params + if (sensor_setting != nullptr) + params_.initializeFromSettings(*static_cast(sensor_setting)); + noise_vec_ = RandomVectorGaussianR(Vector3r::Zero(), params_.noise_sigma); bias_vec_ = RandomVectorR(-params_.noise_bias, params_.noise_bias).next(); diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp index 4ac6fd1469..652cb3257c 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp @@ -31,6 +31,11 @@ struct MagnetometerSimpleParams { real_T update_latency = 0.0f; //sec: from PX4 doc real_T update_frequency = 50; //Hz real_T startup_delay = 0; //sec + + void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings) + { + unused(settings); + } }; diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 0c8041caf7..e2bb0b4492 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -66,24 +66,12 @@ class CarApiBase : public VehicleApiBase { } }; - // sensors for cars - // TODO: Some of this sensor code is duplicate with multi-rotor sensor work. - // But there are subtle structural differences like lack of equivalent MultiRotor - // physics body and associated params. So keeping this code here for now and - // avoiding refactoring for now. - struct EnabledSensors { - bool imu = false; - bool magnetometer = false; - bool gps = false; - bool barometer = false; - bool distance = false; //this causes ray casts so disabled by default - bool lidar = false; //this causes ray casts so disabled by default; lidar_setting - }; - public: - CarApiBase(std::shared_ptr sensor_factory, const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment) + CarApiBase(const AirSimSettings::VehicleSetting* vehicle_setting, + std::shared_ptr sensor_factory, + const Kinematics::State& state, const Environment& environment) { - initialize(sensor_factory, state, environment); + initialize(vehicle_setting, sensor_factory, state, environment); } //default implementation so derived class doesn't have to call on VehicleApiBase @@ -100,7 +88,7 @@ class CarApiBase : public VehicleApiBase { getSensors().update(); } - void reportState(StateReporter& reporter) + void reportState(StateReporter& reporter) override { getSensors().reportState(reporter); } @@ -116,50 +104,29 @@ class CarApiBase : public VehicleApiBase { return sensors_; } - void initialize(std::shared_ptr sensor_factory, const Kinematics::State& state, const Environment& environment) + void initialize(const AirSimSettings::VehicleSetting* vehicle_setting, + std::shared_ptr sensor_factory, + const Kinematics::State& state, const Environment& environment) { sensor_factory_ = sensor_factory; sensor_storage_.clear(); sensors_.clear(); - - EnabledSensors enabledSensors; - addEnabledSensors(enabledSensors); + + addSensorsFromSettings(vehicle_setting); getSensors().initialize(&state, &environment); } - void addEnabledSensors(const EnabledSensors& enabled_sensors) + void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting) { - if (enabled_sensors.imu) - addSensor(SensorBase::SensorType::Imu); - if (enabled_sensors.magnetometer) - addSensor(SensorBase::SensorType::Magnetometer); - if (enabled_sensors.gps) - addSensor(SensorBase::SensorType::Gps); - if (enabled_sensors.barometer) - addSensor(SensorBase::SensorType::Barometer); - if (enabled_sensors.distance) - addSensor(SensorBase::SensorType::Distance); - if (enabled_sensors.lidar) - addSensor(SensorBase::SensorType::Lidar); - } + // use sensors from vehicle settings; if empty list, use default sensors. + // note that the vehicle settings completely override the default sensor "list"; + // there is no piecemeal add/remove/update per sensor. + const std::map>& sensor_settings + = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults; - SensorBase* addSensor(SensorBase::SensorType sensor_type) - { - std::unique_ptr sensor = createSensor(sensor_type); - if (sensor) { - SensorBase* sensor_temp = sensor.get(); - sensor_storage_.push_back(std::move(sensor)); - sensors_.insert(sensor_temp, sensor_type); - return sensor_temp; - } - return nullptr; - } - - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) - { - return sensor_factory_->createSensor(sensor_type); + sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_); } virtual void setCarControls(const CarControls& controls) = 0; diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index fb048a8a7d..24a0011013 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -26,15 +26,6 @@ class MultiRotorParams { {} }; - struct EnabledSensors { - bool imu = true; - bool magnetometer = true; - bool gps = true; - bool barometer = true; - bool distance = false; //this causes ray casts so disabled by default - bool lidar = false; //this causes ray casts so disabled by default; // lidar_setting - }; - struct Params { /*********** required parameters ***********/ uint rotor_count; @@ -52,27 +43,26 @@ class MultiRotorParams { real_T angular_drag_coefficient = linear_drag_coefficient; real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. real_T friction = 0.5f; - EnabledSensors enabled_sensors; RotorParams rotor_params; }; protected: //must override by derived class virtual void setupParams() = 0; - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) = 0; + virtual const SensorFactory* getSensorFactory() = 0; public: //interface virtual std::unique_ptr createMultirotorApi() = 0; virtual ~MultiRotorParams() = default; - virtual void initialize() + virtual void initialize(const AirSimSettings::VehicleSetting* vehicle_setting) { sensor_storage_.clear(); sensors_.clear(); setupParams(); - addEnabledSensors(params_.enabled_sensors); + addSensorsFromSettings(vehicle_setting); } const Params& getParams() const @@ -92,32 +82,15 @@ class MultiRotorParams { return sensors_; } - void addEnabledSensors(const EnabledSensors& enabled_sensors) + void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting) { - if (enabled_sensors.imu) - addSensor(SensorBase::SensorType::Imu); - if (enabled_sensors.magnetometer) - addSensor(SensorBase::SensorType::Magnetometer); - if (enabled_sensors.gps) - addSensor(SensorBase::SensorType::Gps); - if (enabled_sensors.barometer) - addSensor(SensorBase::SensorType::Barometer); - if (enabled_sensors.distance) - addSensor(SensorBase::SensorType::Distance); - if (enabled_sensors.lidar) - addSensor(SensorBase::SensorType::Lidar); - } + // use sensors from vehicle settings; if empty list, use default sensors. + // note that the vehicle settings completely override the default sensor "list"; + // there is no piecemeal add/remove/update per sensor. + const std::map>& sensor_settings + = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults; - SensorBase* addSensor(SensorBase::SensorType sensor_type) - { - std::unique_ptr sensor = createSensor(sensor_type); - if (sensor) { - SensorBase* sensor_temp = sensor.get(); - sensor_storage_.push_back(std::move(sensor)); - sensors_.insert(sensor_temp, sensor_type); - return sensor_temp; - } - return nullptr; + getSensorFactory()->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_); } protected: //static utility functions for derived classes to use diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp index e4d3d319ba..537f1f5bde 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParamsFactory.hpp @@ -35,7 +35,7 @@ class MultiRotorParamsFactory { "Cannot create vehicle config because vehicle name '%s' is not recognized", vehicle_setting->vehicle_name.c_str())); - config->initialize(); + config->initialize(vehicle_setting); return config; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp index 6cec6a25fa..6d389eb5d5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp @@ -81,10 +81,10 @@ namespace msr { namespace airlib { } protected: - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override - { - return sensor_factory_->createSensor(sensor_type); - } + virtual const SensorFactory* getSensorFactory() override + { + return sensor_factory_.get(); + } private: AirSimSettings::MavLinkConnectionInfo connection_info_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp index e3da430dc1..6cf3d1ff36 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp @@ -51,9 +51,9 @@ class Px4MultiRotorParams : public MultiRotorParams { } protected: - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override + virtual const SensorFactory* getSensorFactory() override { - return sensor_factory_->createSensor(sensor_type); + return sensor_factory_.get(); } private: diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp index b1dd20096f..b302d68d14 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp @@ -60,12 +60,11 @@ class SimpleFlightQuadXParams : public MultiRotorParams { //leave everything else to defaults } - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) override + virtual const SensorFactory* getSensorFactory() override { - return sensor_factory_->createSensor(sensor_type); + return sensor_factory_.get(); } - private: vector> sensor_storage_; const AirSimSettings::VehicleSetting* vehicle_setting_; //store as pointer because of derived classes diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index fde22ff6b0..3af07769f2 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -15,6 +15,7 @@ #include "common/SteppableClock.hpp" #include "SimJoyStick/SimJoyStick.h" #include "common/EarthCelestial.hpp" +#include "sensors/lidar/LidarSimple.hpp" #include "DrawDebugHelpers.h" @@ -569,50 +570,53 @@ msr::airlib::VehicleApiBase* ASimModeBase::getVehicleApi(const PawnSimApi::Param // Used for debugging only. void ASimModeBase::drawLidarDebugPoints() { - if (!draw_lidar_debug_points_) // TODO: check airsim settings + // Currently we are checking the sensor-collection instead of sensor-settings. + // Also using variables to optimize not checking the collection if not needed. + if (lidar_checks_done_ && !lidar_draw_debug_points_) return; - if (api_provider_ == nullptr) + if (getApiProvider() == nullptr) return; - std::string vehicle_name = ""; - std::string lidar_name = ""; - - // TODO: check the airsim settings for vehicle and lidar to - // determine if debug points need to drawn. - // For now, show it on the first non-default vehicle-name. - PawnSimApi* vehicle_sim_api = nullptr; - for (auto& api : getApiProvider()->getVehicleSimApis()) { - vehicle_sim_api = static_cast(api); - vehicle_name = vehicle_sim_api->getVehicleName(); - - if (vehicle_name != "") - break; - } - - msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); - - if (api != nullptr) - { - msr::airlib::LidarData lidar_data = api->getLidarData(lidar_name); - - if (lidar_data.point_cloud.size() < 3) - return; - - for (int i = 0; i < lidar_data.point_cloud.size(); i = i + 3) - { - msr::airlib::Vector3r point(lidar_data.point_cloud[i], lidar_data.point_cloud[i + 1], lidar_data.point_cloud[i + 2]); - - FVector uu_point = vehicle_sim_api->getNedTransform().fromLocalNed(point); - - DrawDebugPoint( - this->GetWorld(), - uu_point, - 5, //size - FColor::Green, - true, //persistent (never goes away) - 0.1 //point leaves a trail on moving object - ); + for (auto& sim_api : getApiProvider()->getVehicleSimApis()) { + PawnSimApi* pawn_sim_api = static_cast(sim_api); + std::string vehicle_name = pawn_sim_api->getVehicleName(); + + msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); + if (api != nullptr) { + + msr::airlib::uint count_lidars = api->getSensors().size(msr::airlib::SensorBase::SensorType::Lidar); + + for (msr::airlib::uint i = 0; i < count_lidars; i++) { + // TODO: Is it incorrect to assume LidarSimple here? + const msr::airlib::LidarSimple* lidar = + static_cast(api->getSensors().getByType(msr::airlib::SensorBase::SensorType::Lidar, i)); + if (lidar != nullptr && lidar->getParams().draw_debug_points) { + lidar_draw_debug_points_ = true; + + msr::airlib::LidarData lidar_data = lidar->getOutput(); + + if (lidar_data.point_cloud.size() < 3) + return; + + for (int i = 0; i < lidar_data.point_cloud.size(); i = i + 3) { + msr::airlib::Vector3r point(lidar_data.point_cloud[i], lidar_data.point_cloud[i + 1], lidar_data.point_cloud[i + 2]); + + FVector uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point); + + DrawDebugPoint( + this->GetWorld(), + uu_point, + 5, //size + FColor::Green, + true, //persistent (never goes away) + 0.1 //point leaves a trail on moving object + ); + } + } + } } } + + lidar_checks_done_ = true; } \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 547c3b395d..333f23f44e 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -136,7 +136,8 @@ class AIRSIM_API ASimModeBase : public AActor UPROPERTY() TArray spawned_actors_; //keep refs alive from Unreal GC - bool draw_lidar_debug_points_ = false; // lidar_setting + bool lidar_checks_done_ = false; + bool lidar_draw_debug_points_ = false; private: void setStencilIDs(); diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp index f26488e9ae..030dcba9bf 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp @@ -6,8 +6,9 @@ #include "common/Common.hpp" #include "NedTransform.h" -UnrealDistanceSensor::UnrealDistanceSensor(AActor* actor, const NedTransform* ned_transform) - : actor_(actor), ned_transform_(ned_transform) +UnrealDistanceSensor::UnrealDistanceSensor(AirSimSettings::SensorSetting* sensor_setting, + AActor* actor, const NedTransform* ned_transform) + : DistanceSimple(sensor_setting), actor_(actor), ned_transform_(ned_transform) { } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h index d126f7a4f7..54fdda86a2 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h @@ -10,7 +10,11 @@ class UnrealDistanceSensor : public msr::airlib::DistanceSimple { public: - UnrealDistanceSensor(AActor* actor, const NedTransform* ned_transform); + typedef msr::airlib::AirSimSettings AirSimSettings; + +public: + UnrealDistanceSensor(AirSimSettings::SensorSetting* sensor_setting, + AActor* actor, const NedTransform* ned_transform); protected: virtual msr::airlib::real_T getRayLength(const msr::airlib::Pose& pose) override; diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index 15ac6ddc12..0d1984bccd 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -8,8 +8,9 @@ #include "DrawDebugHelpers.h" // ctor -UnrealLidarSensor::UnrealLidarSensor(AActor* actor, const NedTransform* ned_transform) - : actor_(actor), ned_transform_(ned_transform) +UnrealLidarSensor::UnrealLidarSensor(AirSimSettings::SensorSetting* sensor_setting, + AActor* actor, const NedTransform* ned_transform) + : LidarSimple(sensor_setting), actor_(actor), ned_transform_(ned_transform) { createLasers(); } @@ -26,14 +27,14 @@ void UnrealLidarSensor::createLasers() // calculate verticle angle distance between each laser const float delta_angle = - (params.vertical_FOV_Upper - (params.vertical_FOV_Lower)) / + (params.vertical_FOV_upper - (params.vertical_FOV_lower)) / static_cast(number_of_lasers - 1); // store vertical angles for each laser laser_angles_.clear(); for (auto i = 0u; i < number_of_lasers; ++i) { - const float vertical_angle = params.vertical_FOV_Upper - static_cast(i) * delta_angle; + const float vertical_angle = params.vertical_FOV_upper - static_cast(i) * delta_angle; laser_angles_.emplace_back(vertical_angle); } } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h index edd42df77a..f9cb74ea03 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h @@ -13,7 +13,11 @@ // Thanks to CARLA folks for this. class UnrealLidarSensor : public msr::airlib::LidarSimple { public: - UnrealLidarSensor(AActor* actor, const NedTransform* ned_transform); + typedef msr::airlib::AirSimSettings AirSimSettings; + +public: + UnrealLidarSensor(AirSimSettings::SensorSetting* sensor_setting, + AActor* actor, const NedTransform* ned_transform); protected: virtual void getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose, diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp index 1b5dc02ca3..3f0f0c9e01 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp @@ -16,14 +16,29 @@ std::unique_ptr UnrealSensorFactory::createSensor(msr:: switch (sensor_type) { case SensorBase::SensorType::Distance: - return std::unique_ptr(new UnrealDistanceSensor(actor_, ned_transform_)); + return std::unique_ptr(new UnrealDistanceSensor(NULL /*settings*/, actor_, ned_transform_)); case SensorBase::SensorType::Lidar: - return std::unique_ptr(new UnrealLidarSensor(actor_, ned_transform_)); + return std::unique_ptr(new UnrealLidarSensor(NULL /*settings*/, actor_, ned_transform_)); default: return msr::airlib::SensorFactory::createSensor(sensor_type); } } +std::unique_ptr UnrealSensorFactory::createSensorFromSettings( + AirSimSettings::SensorSetting* sensor_setting) const +{ + using SensorBase = msr::airlib::SensorBase; + + switch (sensor_setting->sensor_type) { + case SensorBase::SensorType::Distance: + return std::unique_ptr(new UnrealDistanceSensor(sensor_setting, actor_, ned_transform_)); + case SensorBase::SensorType::Lidar: + return std::unique_ptr(new UnrealLidarSensor(sensor_setting, actor_, ned_transform_)); + default: + return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting); + } +} + void UnrealSensorFactory::setActor(AActor* actor, const NedTransform* ned_transform) { actor_ = actor; diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h index c69b9a7265..c5b7ebfdf0 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h @@ -10,10 +10,15 @@ #include "GameFramework/Actor.h" class UnrealSensorFactory : public msr::airlib::SensorFactory { +public: + typedef msr::airlib::AirSimSettings AirSimSettings; + public: UnrealSensorFactory(AActor* actor, const NedTransform* ned_transform); void setActor(AActor* actor, const NedTransform* ned_transform); virtual std::unique_ptr createSensor(msr::airlib::SensorBase::SensorType sensor_type) const override; + virtual std::unique_ptr createSensorFromSettings( + AirSimSettings::SensorSetting* sensor_setting) const override; private: AActor* actor_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp index f7d95b570a..b7b5688948 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp @@ -4,8 +4,9 @@ #include "PhysXVehicleManager.h" CarPawnApi::CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, const msr::airlib::GeoPoint& home_geopoint, - std::shared_ptr sensor_factory, const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment) - : msr::airlib::CarApiBase(sensor_factory, state, environment), + const msr::airlib::AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, + const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment) + : msr::airlib::CarApiBase(vehicle_setting, sensor_factory, state, environment), pawn_(pawn), pawn_kinematics_(pawn_kinematics), home_geopoint_(home_geopoint) { movement_ = pawn->GetVehicleMovement(); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h index 95f1d47665..3d12c7467d 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h @@ -11,7 +11,8 @@ class CarPawnApi : public msr::airlib::CarApiBase { typedef msr::airlib::ImageCaptureBase ImageCaptureBase; CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, const msr::airlib::GeoPoint& home_geopoint, - std::shared_ptr sensor_factory, const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment); + const msr::airlib::AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, + const msr::airlib::Kinematics::State& state, const msr::airlib::Environment& environment); virtual void setCarControls(const CarApiBase::CarControls& controls) override; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index 02979a6933..f7552333af 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -22,7 +22,8 @@ void CarPawnSimApi::createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint //create vehicle params std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); vehicle_api_ = std::unique_ptr(new CarPawnApi(pawn, getPawnKinematics(), home_geopoint, - sensor_factory, (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()))); + getVehicleSetting(), sensor_factory, + (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()))); } std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const From 2b21b3acea08c60404199781e88869e394953be1 Mon Sep 17 00:00:00 2001 From: Balinder Malhi Date: Mon, 22 Oct 2018 12:46:50 -0700 Subject: [PATCH 2/2] Add configuration support for sensors via settings json. Incorporated code-review feedback and updated doc files. --- AirLib/include/common/AirSimSettings.hpp | 78 +++++++++--- AirLib/include/sensors/SensorFactory.hpp | 33 ++--- .../sensors/barometer/BarometerSimple.hpp | 7 +- .../sensors/distance/DistanceSimple.hpp | 7 +- AirLib/include/sensors/gps/GpsSimple.hpp | 7 +- AirLib/include/sensors/imu/ImuSimple.hpp | 7 +- AirLib/include/sensors/lidar/LidarSimple.hpp | 7 +- .../sensors/lidar/LidarSimpleParams.hpp | 2 + .../magnetometer/MagnetometerSimple.hpp | 7 +- .../vehicles/multirotor/MultiRotorParams.hpp | 2 +- .../mavlink/ArduCopterSoloParams.hpp | 2 +- .../firmwares/mavlink/Px4MultiRotorParams.hpp | 2 +- .../simple_flight/SimpleFlightQuadXParams.hpp | 2 +- .../UnrealSensors/UnrealDistanceSensor.cpp | 4 +- .../UnrealSensors/UnrealDistanceSensor.h | 2 +- .../UnrealSensors/UnrealLidarSensor.cpp | 4 +- .../Source/UnrealSensors/UnrealLidarSensor.h | 2 +- .../UnrealSensors/UnrealSensorFactory.cpp | 23 +--- .../UnrealSensors/UnrealSensorFactory.h | 3 +- docs/lidar.md | 86 ++++++------- docs/sensors.md | 120 ++++++++++++++++++ 21 files changed, 263 insertions(+), 144 deletions(-) create mode 100644 docs/sensors.md diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 4e5bb98b1a..58ac1bfc1b 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -1084,23 +1084,63 @@ struct AirSimSettings { clock_speed = settings_json.getFloat("ClockSpeed", 1.0f); } - static void initializeLidarSetting(LidarSetting* lidar_setting, const Settings& settings_json) + static void initializeBarometerSetting(BarometerSetting& barometer_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); + unused(barometer_setting); + unused(settings_json); - 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); + //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); + lidar_setting.position = createVectorSetting(settings_json, lidar_setting.position); + lidar_setting.rotation = createRotationSetting(settings_json, lidar_setting.rotation); } static std::unique_ptr createSensorSetting( - SensorBase::SensorType sensor_type, const std::string sensor_name, + SensorBase::SensorType sensor_type, const std::string& sensor_name, bool enabled) { std::unique_ptr sensor_setting; @@ -1125,7 +1165,7 @@ struct AirSimSettings { sensor_setting = std::unique_ptr(new LidarSetting()); break; default: - break; + throw std::invalid_argument("Unexpected sensor type"); } sensor_setting->sensor_type = sensor_type; @@ -1141,25 +1181,30 @@ struct AirSimSettings { switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Barometer: + initializeBarometerSetting(*static_cast(sensor_setting), settings_json); break; case SensorBase::SensorType::Imu: + initializeImuSetting(*static_cast(sensor_setting), settings_json); break; case SensorBase::SensorType::Gps: + initializeGpsSetting(*static_cast(sensor_setting), settings_json); break; case SensorBase::SensorType::Magnetometer: + initializeMagnetometerSetting(*static_cast(sensor_setting), settings_json); break; case SensorBase::SensorType::Distance: + initializeDistanceSetting(*static_cast(sensor_setting), settings_json); break; case SensorBase::SensorType::Lidar: - initializeLidarSetting(static_cast(sensor_setting), settings_json); + initializeLidarSetting(*static_cast(sensor_setting), settings_json); break; default: - break; + 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, + static void loadSensorSettings( const Settings& settings_json, const std::string& collectionName, std::map>& sensors) { msr::airlib::Settings sensors_child; @@ -1193,6 +1238,9 @@ struct AirSimSettings { 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 diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp index ad9cec54c2..1fffe86411 100644 --- a/AirLib/include/sensors/SensorFactory.hpp +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -17,39 +17,22 @@ namespace msr { namespace airlib { class SensorFactory { public: - // creates one sensor - // TODO: Can we remove this function alltogether and require the one that accepts settings only? - virtual std::unique_ptr createSensor(SensorBase::SensorType sensor_type) const - { - switch (sensor_type) { - case SensorBase::SensorType::Imu: - return std::unique_ptr(new ImuSimple()); - case SensorBase::SensorType::Magnetometer: - return std::unique_ptr(new MagnetometerSimple()); - case SensorBase::SensorType::Gps: - return std::unique_ptr(new GpsSimple()); - case SensorBase::SensorType::Barometer: - return std::unique_ptr(new BarometerSimple()); - default: - return std::unique_ptr(); - } - } // creates one sensor from settings virtual std::unique_ptr createSensorFromSettings( - AirSimSettings::SensorSetting* sensor_setting) const + const AirSimSettings::SensorSetting* sensor_setting) const { switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Imu: - return std::unique_ptr(new ImuSimple(sensor_setting)); + return std::unique_ptr(new ImuSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Magnetometer: - return std::unique_ptr(new MagnetometerSimple(sensor_setting)); + return std::unique_ptr(new MagnetometerSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Gps: - return std::unique_ptr(new GpsSimple(sensor_setting)); + return std::unique_ptr(new GpsSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Barometer: - return std::unique_ptr(new BarometerSimple(sensor_setting)); + return std::unique_ptr(new BarometerSimple(*static_cast(sensor_setting))); default: - return std::unique_ptr(); + throw new std::invalid_argument("Unexpected sensor type"); } } @@ -59,8 +42,8 @@ class SensorFactory { SensorCollection& sensors, vector>& sensor_storage) const { - for (auto& sensor_setting_pair : sensors_settings) { - AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get(); + 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) diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index ea78fcdb36..3c2c24c648 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -18,12 +18,11 @@ namespace msr { namespace airlib { class BarometerSimple : public BarometerBase { public: - BarometerSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : BarometerBase(sensor_setting != nullptr ? sensor_setting->sensor_name: "") + BarometerSimple(const AirSimSettings::BarometerSetting& setting = AirSimSettings::BarometerSetting()) + : BarometerBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); //GM process that would do random walk for pressure factor pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0); diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index cc12f0e9e7..b1cfb27c0f 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -16,12 +16,11 @@ namespace msr { namespace airlib { class DistanceSimple : public DistanceBase { public: - DistanceSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : DistanceBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") + DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting()) + : DistanceBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma); //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f); diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 718fcafa5f..a80ddba7a2 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -17,12 +17,11 @@ namespace msr { namespace airlib { class GpsSimple : public GpsBase { public: //methods - GpsSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : GpsBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") + GpsSimple(const AirSimSettings::GpsSetting& setting = AirSimSettings::GpsSetting()) + : GpsBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); //initialize frequency limiter freq_limiter_.initialize(params_.update_frequency, params_.startup_delay); diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 6346ebca20..34060ab8cb 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -14,12 +14,11 @@ namespace msr { namespace airlib { class ImuSimple : public ImuBase { public: //constructors - ImuSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : ImuBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") + ImuSimple(const AirSimSettings::ImuSetting& setting = AirSimSettings::ImuSetting()) + : ImuBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); gyro_bias_stability_norm = params_.gyro.bias_stability / sqrt(params_.gyro.tau); accel_bias_stability_norm = params_.accel.bias_stability / sqrt(params_.accel.tau); diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp index 42b8f4852b..0a61524ef1 100644 --- a/AirLib/include/sensors/lidar/LidarSimple.hpp +++ b/AirLib/include/sensors/lidar/LidarSimple.hpp @@ -15,12 +15,11 @@ namespace msr { namespace airlib { class LidarSimple : public LidarBase { public: - LidarSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : LidarBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") + LidarSimple(const AirSimSettings::LidarSetting& setting = AirSimSettings::LidarSetting()) + : LidarBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); //initialize frequency limiter freq_limiter_.initialize(params_.update_frequency, params_.startup_delay); diff --git a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp index 6df42277ba..7123bc090e 100644 --- a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp +++ b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp @@ -42,6 +42,8 @@ struct LidarSimpleParams { points_per_second = settings.points_per_second; horizontal_rotation_frequency = settings.horizontal_rotation_frequency; + // By default, for multirotors the lidars FOV point downwards; + // for cars, the lidars FOV is more forward facing. vertical_FOV_upper = settings.vertical_FOV_upper; if (std::isnan(vertical_FOV_upper)) { if (simmode_name == "Multirotor") diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 47e67e6dec..8106b4d2e9 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -17,12 +17,11 @@ namespace msr { namespace airlib { class MagnetometerSimple : public MagnetometerBase { public: - MagnetometerSimple(AirSimSettings::SensorSetting* sensor_setting = nullptr) - : MagnetometerBase(sensor_setting != nullptr ? sensor_setting->sensor_name : "") + MagnetometerSimple(const AirSimSettings::MagnetometerSetting& setting = AirSimSettings::MagnetometerSetting()) + : MagnetometerBase(setting.sensor_name) { // initialize params - if (sensor_setting != nullptr) - params_.initializeFromSettings(*static_cast(sensor_setting)); + params_.initializeFromSettings(setting); noise_vec_ = RandomVectorGaussianR(Vector3r::Zero(), params_.noise_sigma); bias_vec_ = RandomVectorR(-params_.noise_bias, params_.noise_bias).next(); diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index 24a0011013..b398d0c829 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -49,7 +49,7 @@ class MultiRotorParams { protected: //must override by derived class virtual void setupParams() = 0; - virtual const SensorFactory* getSensorFactory() = 0; + virtual const SensorFactory* getSensorFactory() const = 0; public: //interface virtual std::unique_ptr createMultirotorApi() = 0; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp index 6d389eb5d5..3424844619 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloParams.hpp @@ -81,7 +81,7 @@ namespace msr { namespace airlib { } protected: - virtual const SensorFactory* getSensorFactory() override + virtual const SensorFactory* getSensorFactory() const override { return sensor_factory_.get(); } diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp index 6cf3d1ff36..836f8943b2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp @@ -51,7 +51,7 @@ class Px4MultiRotorParams : public MultiRotorParams { } protected: - virtual const SensorFactory* getSensorFactory() override + virtual const SensorFactory* getSensorFactory() const override { return sensor_factory_.get(); } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp index b302d68d14..29c0b24937 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp @@ -60,7 +60,7 @@ class SimpleFlightQuadXParams : public MultiRotorParams { //leave everything else to defaults } - virtual const SensorFactory* getSensorFactory() override + virtual const SensorFactory* getSensorFactory() const override { return sensor_factory_.get(); } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp index 030dcba9bf..c0202b701e 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.cpp @@ -6,9 +6,9 @@ #include "common/Common.hpp" #include "NedTransform.h" -UnrealDistanceSensor::UnrealDistanceSensor(AirSimSettings::SensorSetting* sensor_setting, +UnrealDistanceSensor::UnrealDistanceSensor(const AirSimSettings::DistanceSetting& setting, AActor* actor, const NedTransform* ned_transform) - : DistanceSimple(sensor_setting), actor_(actor), ned_transform_(ned_transform) + : DistanceSimple(setting), actor_(actor), ned_transform_(ned_transform) { } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h index 54fdda86a2..5898b7cd6d 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealDistanceSensor.h @@ -13,7 +13,7 @@ class UnrealDistanceSensor : public msr::airlib::DistanceSimple { typedef msr::airlib::AirSimSettings AirSimSettings; public: - UnrealDistanceSensor(AirSimSettings::SensorSetting* sensor_setting, + UnrealDistanceSensor(const AirSimSettings::DistanceSetting& setting, AActor* actor, const NedTransform* ned_transform); protected: diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index 0d1984bccd..0f39f11c91 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -8,9 +8,9 @@ #include "DrawDebugHelpers.h" // ctor -UnrealLidarSensor::UnrealLidarSensor(AirSimSettings::SensorSetting* sensor_setting, +UnrealLidarSensor::UnrealLidarSensor(const AirSimSettings::LidarSetting& setting, AActor* actor, const NedTransform* ned_transform) - : LidarSimple(sensor_setting), actor_(actor), ned_transform_(ned_transform) + : LidarSimple(setting), actor_(actor), ned_transform_(ned_transform) { createLasers(); } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h index f9cb74ea03..6d8a74dabd 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h @@ -16,7 +16,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple { typedef msr::airlib::AirSimSettings AirSimSettings; public: - UnrealLidarSensor(AirSimSettings::SensorSetting* sensor_setting, + UnrealLidarSensor(const AirSimSettings::LidarSetting& setting, AActor* actor, const NedTransform* ned_transform); protected: diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp index 3f0f0c9e01..4686633210 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp @@ -9,31 +9,18 @@ UnrealSensorFactory::UnrealSensorFactory(AActor* actor, const NedTransform* ned_ setActor(actor, ned_transform); } - -std::unique_ptr UnrealSensorFactory::createSensor(msr::airlib::SensorBase::SensorType sensor_type) const -{ - using SensorBase = msr::airlib::SensorBase; - - switch (sensor_type) { - case SensorBase::SensorType::Distance: - return std::unique_ptr(new UnrealDistanceSensor(NULL /*settings*/, actor_, ned_transform_)); - case SensorBase::SensorType::Lidar: - return std::unique_ptr(new UnrealLidarSensor(NULL /*settings*/, actor_, ned_transform_)); - default: - return msr::airlib::SensorFactory::createSensor(sensor_type); - } -} - std::unique_ptr UnrealSensorFactory::createSensorFromSettings( - AirSimSettings::SensorSetting* sensor_setting) const + const AirSimSettings::SensorSetting* sensor_setting) const { using SensorBase = msr::airlib::SensorBase; switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Distance: - return std::unique_ptr(new UnrealDistanceSensor(sensor_setting, actor_, ned_transform_)); + return std::unique_ptr(new UnrealDistanceSensor( + *static_cast(sensor_setting), actor_, ned_transform_)); case SensorBase::SensorType::Lidar: - return std::unique_ptr(new UnrealLidarSensor(sensor_setting, actor_, ned_transform_)); + return std::unique_ptr(new UnrealLidarSensor( + *static_cast(sensor_setting), actor_, ned_transform_)); default: return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting); } diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h index c5b7ebfdf0..d0758705bb 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h @@ -16,9 +16,8 @@ class UnrealSensorFactory : public msr::airlib::SensorFactory { public: UnrealSensorFactory(AActor* actor, const NedTransform* ned_transform); void setActor(AActor* actor, const NedTransform* ned_transform); - virtual std::unique_ptr createSensor(msr::airlib::SensorBase::SensorType sensor_type) const override; virtual std::unique_ptr createSensorFromSettings( - AirSimSettings::SensorSetting* sensor_setting) const override; + const AirSimSettings::SensorSetting* sensor_setting) const override; private: AActor* actor_; diff --git a/docs/lidar.md b/docs/lidar.md index 627d86e540..3756b1aa09 100644 --- a/docs/lidar.md +++ b/docs/lidar.md @@ -2,63 +2,50 @@ AirSim supports Lidar for multirotors and cars. -The enablement of lidar and the configuration currently requires manual changes to the code; but that will be fixed soon. +The enablement of lidar and the other lidar settings can be configured via AirSimSettings json. +Please see [general sensors](sensors.md) for information on configruation of general/shared sensor settings. ## Enabling lidar on a vehicle -For multirotors, please update the EnabledSensors struct in file [MultiRotorParams.hpp](../AirLib/include/vehicles/multirotor) and set lidar to `true`. - -```cpp - -// File location: https://github.com/Microsoft/AirSim/blob/master/AirLib/include/vehicles/multirotor/MultiRotor.hpp - -struct EnabledSensors { - bool imu = true; - bool magnetometer = true; - bool gps = true; - bool barometer = true; - bool distance = false; - bool lidar = false; //set this to true - }; +* By default, lidars are not enabled. To enable lidar, set the SensorType and Enabled attributes in settings json. ``` - -For cars, please update the EnabledSensors struct in file [CarApiBase.hpp](../AirLib/include/vehicles/car/api) and set lidar to `true`. - -```cpp - -// File location: https://github.com/Microsoft/AirSim/blob/master/AirLib/include/vehicles/car/api/CarApiBase.hpp - -struct EnabledSensors { - bool imu = false; - bool magnetometer = false; - bool gps = false; - bool barometer = false; - bool distance = false; - bool lidar = true; //set this to true - }; + "Lidar1": { + "SensorType": 6, + "Enabled" : true, ``` +* Multiple lidars can be enabled on a vehicle. ## Lidar configuration -Update the Lidar configuration as needed in file [LidarSimpleParams.hpp](../AirLib/include/sensors/lidar). -The following parameters can be configured right now: - -Parameter | Description ------------------------------- | ------------ -number_of_channels | Number of channels/lasers of the lidar -range | Range, in meters -points_per_second | Number of points captured per second -horizontal_rotation_frequency | Rotations per second -vertical_FOV_Upper | Vertical FOV upper limit for the lidar, in degrees -vertical_FOV_Lower | Vertical FOV lower limit for the lidar, in degrees -relative_pose | Position and rotation of the lidar relative to the vehicle +The following parameters can be configured right now via settings json. + +Parameter | Description +--------------------------| ------------ +NumberOfChannels | Number of channels/lasers of the lidar +Range | Range, in meters +PointsPerSecond | Number of points captured per second +RotationsPerSecond | Rotations per second +VerticalFOVUpper | Vertical FOV upper limit for the lidar, in degrees +VerticalFOVLower | Vertical FOV lower limit for the lidar, in degrees +X Y Z | Position of the lidar relative to the vehicle (in NED, in meters) +Roll Pitch Yaw | Roation of the lidar relative to the vehicle (in degrees) + +e.g., +``` + "Lidar1": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 16, + "PointsPerSecond": 10000, + "X": 0, "Y": 0, "Z": -1, +``` ## Server side visualization for debugging -Be default, the lidar points are not drawn on the viewport. To enable the drawing of hit laser points on the viewport, please update the code in file [SimModeBase.h](../Unreal/Plugins/AirSim/Source/SimMode). -Set `draw_lidar_debug_points_` to true. - -```cpp - -bool draw_lidar_debug_points_ = false; // set this to true - +Be default, the lidar points are not drawn on the viewport. To enable the drawing of hit laser points on the viewport, please enable setting 'DrawDebugPoints' via settings json. +e.g., +``` + "Lidar1": { + ... + "DrawDebugPoints": true + }, ``` ## Client API @@ -72,5 +59,4 @@ Use `getLidarData()` API to retrieve the Lidar data. [car_lidar.py](../PythonClient/car) ## Coming soon -* Configuration of lidar parameters via AirSim settings. * Visualization of lidar data on client side. \ No newline at end of file diff --git a/docs/sensors.md b/docs/sensors.md new file mode 100644 index 0000000000..ea738d355a --- /dev/null +++ b/docs/sensors.md @@ -0,0 +1,120 @@ +# Sensors in AirSim + +AirSim currently supports the following sensors: +* Camera +* Imu +* Magnetometer +* Gps +* Barometer +* Distance +* Lidar + +The cameras are currently configured a bit differently than other sensors. The camera configuration and apis are covered in other documents, e.g., [general settings](settings.md) and [image API](image_apis.md). + +This document focuses on the configuration of other sensors. + +## Default sensors + +If not sensors are specified in the settings json, the the following sensors are enabled by default based on the simmode. + +### Multirotor +* Imu +* Magnetometer +* Gps +* Barometer +### Car +* Gps +### ComputerVision +* None + +Please see 'createDefaultSensorSettings' method in [AirSimSettings.hpp](../AirLib/include/common/) + +## Configuration of Default Sensor list + +A default sensor list can be configured in settings json. +e.g., +``` + "DefaultSensors": { + "Barometer": { + "SensorType": 1, + "Enabled" : true + }, + "Gps": { + "SensorType": 1, + "Enabled" : true + }, + "Lidar1": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 16, + "PointsPerSecond": 10000 + }, + "Lidar2": { + "SensorType": 6, + "Enabled" : false, + "NumberOfChannels": 4, + "PointsPerSecond": 10000 + } + }, +``` + +## Configuration of vehicle specific sensor settings + +A vehicle specific sensor list can be specified in the vehicle settings part of the json. +e.g., + +``` + "Vehicles": { + + "Drone1": { + "VehicleType": "simpleflight", + "AutoCreate": true, + ... + "Sensors": { + "MyLidar1": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 16, + "PointsPerSecond": 10000, + "X": 0, "Y": 0, "Z": -1, + "DrawDebugPoints": true + }, + "MyLidar2": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 4, + "PointsPerSecond": 10000, + "X": 0, "Y": 0, "Z": -1, + "DrawDebugPoints": true + } + } + } + } +``` + +If a vehicle provides its sensor list, it must provide the whole list. Selective add/remove/update of the default sensor list is NOT supported. + +## Configuration of sensor settings + +### Shared settings +There are two shared settings: +* SensorType + An integer representing the sensor-type [SensorBase.hpp](../AirLib/include/sensors/) +``` + enum class SensorType : uint { + Barometer = 1, + Imu = 2, + Gps = 3, + Magnetometer = 4, + Distance = 5, + Lidar = 6 + }; +``` +* Enabled + Boolean + +### Sensor specific settings +Each sensor-type has its own set of settings as well. Please see [lidar](lidar.md) for example of Lidar specific settings. + +## Sensor APIs +Each sensor-type has its own set of APIs currently. Please see [lidar](lidar.md) for example of Lidar specific APIs.